A UWB-based indoor SLAM initialization positioning method

Abstract

The application discloses a kind of indoor SLAM initialization positioning methods based on UWB, comprising the following steps: step 1, by UWB drive SLAM initialization, and realize the automatic matching of UWB global coordinate system and SLAM laser local coordinate system;Step 2, by SLAM algorithm based on the independent execution of self-positioning in sand table and global map construction of aligned global coordinate system;Step 3, UWB positioning module is woken up every interval preset time, current absolute check coordinate is collected, and the current global coordinate of trolley calculated by SLAM algorithm is compared, and real-time positioning deviation and cumulative average deviation are calculated for precision check;Step 4, judge whether to meet the operation end condition, not meet then repeat step 2-3, meet then end operation.The application provides the absolute initial position under sand table global coordinate system for SLAM by UWB, combined with automatic coordinate system matching, SLAM initialization pose error is reduced from ≥20cm to ≤5cm, and the mapping misplacement caused by pure SLAM initialization drift is completely eliminated.

Description

Technical Field

[0001] This invention belongs to the field of simultaneous localization and mapping technology, and particularly relates to an indoor SLAM initialization localization method based on UWB. Background Technology

[0002] Indoor autonomous driving sandbox vehicles are the core carrier for teaching the principles of SLAM (Simultaneous Localization and Mapping) in autonomous driving. In sandbox scenarios, laser SLAM is the mainstream method for localization and mapping teaching. However, pure laser SLAM lacks absolute position reference. During the initialization phase, due to the lack of initial position anchoring in a global coordinate system, initial pose drift is prone to occur, leading to map building misalignment and decreased positioning accuracy. In the field of outdoor autonomous driving, RTK is often used to provide a global initial position for SLAM. However, indoor sandboxes have no satellite signals. UWB, as a high-precision wireless positioning technology for indoor use, can be compared with the role of outdoor RTK to provide an absolute initial position in a global coordinate system for sandbox vehicle SLAM. This solves the teaching pain point of initial drift in pure SLAM and can also intuitively demonstrate the "impact of global initial position on SLAM mapping / localization", thus improving the teaching effect.

[0003] Existing UWB and SLAM integration solutions are mostly geared towards continuous localization and mapping tasks for general indoor robots or mobile platforms. They typically employ real-time fusion or filtered fusion methods with full UWB participation. They lack a phased operation mechanism specifically designed for indoor autonomous driving sandbox vehicle teaching scenarios, including "absolute pose anchoring during initialization, autonomous closed-loop operation during runtime, and low-frequency wake-up during verification." Specifically, this includes the following shortcomings:

[0004] The full integration of UWB and SLAM without distinguishing between the initialization phase and the normal driving phase consumes a large amount of the low-computing-power chip resources of the vehicle, with a data fusion delay of ≥20ms. Moreover, the meaningless full integration fails to highlight the core teaching significance of "initial position".

[0005] Without automated coordinate system matching logic, the global coordinate system of the UWB sandbox and the local laser coordinate system of SLAM need to be manually calibrated and matched, which is complicated and students cannot complete the teaching practice independently.

[0006] The fusion algorithm adopts an industrial-grade complex filtering strategy, but it has not been optimized for lightweighting in small sand table scenarios (1-10㎡) and is not suitable for low-cost processing chips for small vehicles. Summary of the Invention

[0007] To address the aforementioned shortcomings of existing technologies, this invention designs an indoor SLAM initialization and localization method based on UWB. The core of this method lies in utilizing UWB to provide the SLAM with an absolute initial pose in the global coordinate system of a sand table, and to achieve automatic alignment between the two coordinate systems. Unlike indoor mapping schemes that continuously use UWB for providing initial values ​​for front-end matching, adding constraints to the back-end pose graph, or assisting with loop closure, this invention limits UWB to absolute pose anchoring during the initialization phase and low-frequency verification during the operational phase. After initialization, it no longer participates in SLAM front-end matching and back-end graph optimization.

[0008] This invention discloses an indoor SLAM initialization localization method based on UWB, which includes the following steps:

[0009] Step 1: Initialize SLAM using UWB-driven SLAM, obtain the absolute initial pose of the vehicle in the UWB global coordinate system of the sand table, and achieve automatic matching between the UWB global coordinate system and the SLAM laser local coordinate system.

[0010] Step 2: After initialization, the SLAM algorithm independently performs autonomous localization and global map construction within the sandbox based on the aligned global coordinate system.

[0011] Step 3: Wake up the UWB positioning module at preset intervals, collect the current absolute verification coordinates, and compare them with the current global coordinates of the vehicle calculated by the SLAM algorithm. Calculate the real-time positioning deviation and the cumulative average deviation. The deviation is only used for accuracy verification and is not fed back to the SLAM algorithm for real-time pose correction.

[0012] Step 4: Determine if the conditions for ending the run are met. If not, repeat steps 2 to 3. If the conditions are met, end the run.

[0013] Furthermore, step 1 also includes the following steps:

[0014] Step 11: Activate the UWB positioning module and the laser scanning module. The UWB positioning module acquires the absolute initial coordinates of the vehicle body center in the UWB global coordinate system of the sand table in real time. Simultaneously, the initial heading angle between the car's driving direction and the positive X-axis direction of the UWB global coordinate system is collected. ;

[0015] Step 12: The laser scanning module performs a comprehensive scan of the environment surrounding the sand table, identifying and acquiring the coordinates of three preset non-collinear reference feature points in the SLAM laser local coordinate system. ;

[0016] Step 13: Automatic alignment of the two coordinate systems is completed using the rigid body transformation matrix T;

[0017] Step 14: Calculate the average matching error. When e ≤ 5cm, the coordinate system is determined to be successfully matched, the UWB positioning module enters sleep mode, and SLAM initialization is completed. If e > 5cm, the laser feature points are re-acquired and calculated until the matching is successful.

[0018] Furthermore, in step 13, the rigid body transformation matrix is:

[0019] T= ;

[0020] in The initial heading angle acquired by UWB. The initial coordinates for UWB acquisition.

[0021] Furthermore, in step 13, the local coordinates of the three non-collinear reference feature points are substituted into the transformation formula to complete the alignment:

[0022] ;

[0023] The x-coordinate in the UWB global coordinate system The ordinate is the vertical coordinate in the UWB global coordinate system; Let x be the x-coordinate in the local coordinate system of SLAM laser. The vertical coordinate is the ordinate in the local coordinate system of the SLAM laser.

[0024] Solve its verification coordinates in the UWB global coordinate system. .

[0025] Furthermore, in step 14, the average matching error is:

[0026] e= ;

[0027] e represents the average matching error. Let i be the x-coordinate of the number i in the UWB global coordinate system. Let i be the ordinate of the number i in the UWB global coordinate system. Verify the x-coordinate of index i in the UWB global coordinate system. Verify the ordinate of point i in the UWB global coordinate system, where i is the number of the non-collinear reference feature point.

[0028] Furthermore, in step 2, the laser scanning module continuously collects point cloud data of the sand table environment in real time, and accurately extracts environmental feature points through the neighborhood point cloud curvature extraction algorithm. The curvature calculation adopts the neighborhood point distance deviation ratio method. By using the average distance from the neighborhood point to the target feature point, the sum of squares of the deviations between each neighborhood point and the average distance is calculated, and then divided by the product of the number of neighborhood points and the average distance to achieve accurate curvature calculation.

[0029] Curvature calculation formula:

[0030] ;

[0031] Average distance formula:

[0032] ;

[0033] In the formula, Let i be the set of neighborhood points of the target feature point i. The number of neighboring points. Let j be the distance from the neighboring point j to the laser scanning module. The mean distance between neighboring points;

[0034] curvature The larger the curvature, the more it tends towards the corner point; The smaller the value, the more it tends to be a planar point;

[0035] Based on the initialized UWB global coordinate system, the real-time pose of the vehicle at time t is calculated using the pose update formula. The pose update formula is:

[0036] ;

[0037] in Δx and Δy represent the translational changes of the current time relative to the previous time, and Δθ represents the rotational changes.

[0038] When the car travels to the scanned area in the sand table, loop closure detection is completed by feature point similarity matching. If a loop is detected, the graph optimization algorithm is used to optimize the existing map and pose trajectory to eliminate small drifts during the driving process. The graph optimization target is calculated by the sum of squared pose constraint deviations.

[0039] ;

[0040] In the formula, Let be the set of optimal poses after graph optimization. For the first The information matrix corresponding to the feature point matching constraints at time j and time j. Let ξ be the set of pose constraint edges, and ξ be the set of vehicle poses. The feature point matching constraint between time i and time j;

[0041] Finally, the laser point cloud data at all times are transformed into the UWB global coordinate system through the real-time pose set ξ and the rigid body transformation matrix T calculated in step 13. The data are then stitched together to form a complete global map of the sand table and output to the car processing module in real time for subsequent data review and accuracy verification.

[0042] Furthermore, in step 3, during the calculation of real-time positioning deviation, the UWB positioning module is woken up at preset time intervals, and the UWB positioning module collects the current absolute verification coordinates in the UWB global coordinate system. ;

[0043] Simultaneously, extract the current global coordinates of the vehicle calculated by the SLAM algorithm. Real-time positioning deviation is calculated using a formula. :

[0044] ;

[0045] Real-time deviation reflects the SLAM positioning accuracy at the current moment;

[0046] Simultaneously calculate the cumulative average deviation:

[0047] ;

[0048] In the formula, N represents the number of verifications and the cumulative average deviation. The overall positioning accuracy of the algorithm is reflected in its accuracy verification.

[0049] The beneficial effects achieved by this invention are:

[0050] ① Completely solve the SLAM initialization drift problem: By providing SLAM with the absolute initial position in the global coordinate system of the sandbox through UWB, combined with automatic coordinate system matching, the SLAM initial pose error is reduced from ≥20cm to ≤5cm, completely eliminating the mapping misalignment caused by pure SLAM initialization drift;

[0051] ② Lightweight adaptation to car hardware: Adopting a phased fusion strategy, only calling UWB data during the initialization phase, and SLAM autonomous closed loop during normal driving phase. After optimization of the algorithm in the sand table scenario, the computing power consumption is ≤30% and the data processing latency is ≤10ms, which is fully adapted to the low computing power processing module of the car.

[0052] ③ Automated operation, simplified implementation process: Derive the automatic coordinate system transformation formula to achieve one-click matching between the UWB global coordinate system and the SLAM local coordinate system, eliminating the need for manual calibration and lowering the implementation threshold;

[0053] ④ Strong adaptability and high versatility: The algorithm is compatible with various sand table cars and general processing chips, requiring no modification to the car. It can be adapted to indoor autonomous driving sand tables of different specifications and has strong promotion potential.

[0054] ⑤ Stable operation and controllable accuracy: The UWB positioning function only works during the initialization and low-frequency verification phases, and remains dormant at other times, which greatly reduces power consumption. At the same time, the algorithm accuracy is monitored in real time through low-frequency verification, ensuring the stability of positioning and mapping. Attached Figure Description

[0055] Figure 1 This is a flowchart illustrating an indoor SLAM initialization and positioning method based on UWB according to the present invention. Detailed Implementation

[0056] The present invention will be further described below with reference to specific embodiments, and the advantages and features of the present invention will become clearer as a result. However, these embodiments are merely exemplary and do not constitute any limitation on the scope of the present invention. Those skilled in the art should understand that modifications or substitutions can be made to the details and form of the technical solutions of the present invention without departing from the spirit and scope of the present invention, but all such modifications and substitutions fall within the protection scope of the present invention.

[0057] like Figure 1 As shown, this invention is an indoor SLAM initialization and positioning method based on UWB. Its core is a phased fusion strategy of "UWB initialization and anchoring + SLAM autonomous loop closure + UWB low-frequency verification." The overall approach is a phased fusion algorithm module. The algorithm derives multiple core formulas to achieve functions such as automatic coordinate system matching, pose calculation, and error verification, and is compatible with various general-purpose processing chips for sand table vehicles. The specific process of this method includes the following steps:

[0058] Step 1, UWB-driven SLAM initialization (core anchoring), provides the SLAM algorithm with the absolute initial pose in the global coordinate system of the sandbox, and completes the automatic matching between the UWB global coordinate system and the SLAM laser local coordinate system, eliminating the pure SLAM initialization drift problem at its source. After this step, the UWB module is no longer used as a continuous input for inter-frame matching in the SLAM front end, nor as a continuous observation constraint for the SLAM back end pose map, but only retains the low-frequency wake-up verification function.

[0059] The specific steps are as follows:

[0060] Step 11: First, start the UWB positioning module and laser scanning module on the vehicle, ensuring that the two modules work synchronously and that data transmission is normal; the UWB positioning module collects the absolute initial coordinates of the vehicle's center in the UWB global coordinate system of the sand table in real time. Simultaneously, the initial heading angle between the car's driving direction and the positive X-axis direction of the UWB global coordinate system is collected. (Counterclockwise is positive, with a value range of 0~2π and an accuracy of 0.1°).

[0061] Step 12: The laser scanning module performs a comprehensive scan of the environment surrounding the sand table, accurately identifying and collecting the coordinates of three pre-defined non-collinear reference feature points (pre-calibrated within the sand table and distributed across different areas to ensure matching accuracy) in the SLAM laser local coordinate system. ;

[0062] Step 13: Using the rigid body transformation matrix T derived in this invention, complete the automatic alignment of the two coordinate systems. The rigid body transformation matrix is:

[0063] T=

[0064] in The initial heading angle (rotation factor) acquired for UWB. The initial coordinates (translation factor) collected for UWB are contained in a matrix that only includes translation and rotation, without scaling, and is adapted to a sandbox-like scene.

[0065] Substitute the local coordinates of the three reference feature points into the transformation formula:

[0066]

[0067] Solve its verification coordinates in the UWB global coordinate system. ;

[0068] Step 14, calculate the average matching error:

[0069] e=

[0070] When e ≤ 5cm, the coordinate system is considered to be successfully matched, the UWB positioning module enters sleep mode (stops data transmission and releases computing power), and SLAM initialization is completed; if e > 5cm, laser feature points are re-acquired and calculated until the matching is successful.

[0071] Step 2 is the autonomous SLAM operation phase after initialization. In this phase, the vehicle performs conventional laser SLAM localization and map building based on the aligned global coordinate system. The UWB module no longer participates in the front-end inter-frame matching, back-end pose graph constraint addition, and loop closure detection processes.

[0072] The specific operation is as follows: The laser scanning module continuously collects point cloud data of the sand table environment in real time (the collection frequency is 10Hz, which is adapted to low computing power chips to avoid data accumulation). The environmental feature points (including corner points and planar points, corner points are used for localization, and planar points are used for mapping) are accurately extracted through the neighborhood point cloud curvature extraction algorithm. The curvature calculation adopts the neighborhood point distance deviation ratio method. Its core logic is: by using the average distance from the neighborhood point to the target feature point, the sum of the squares of the deviations of each neighborhood point from the average distance is calculated, and then divided by the product of the number of neighborhood points and the average distance to achieve accurate curvature calculation.

[0073] Curvature calculation formula:

[0074]

[0075] Average distance formula:

[0076]

[0077] In the formula, Let i be the set of neighborhood points of the target feature point i. The number of neighboring points. Let j be the distance from the neighboring point j to the laser scanning module. This represents the average distance between neighboring points. The larger it is, the more it tends to be at the corner. The smaller the value, the more it tends to be a planar point.

[0078] Based on the UWB global coordinate system initialized in step 1, the real-time pose of the vehicle is calculated using the pose update formula. (where t is the current time), the pose update formula is:

[0079]

[0080] in Δx and Δy represent the translational changes of the current time relative to the previous time, and Δθ represents the rotational changes.

[0081] When the car travels to the scanned area in the sand table, loop closure detection is completed by feature point similarity matching (the similarity threshold is set to 0.8 to avoid false matching). If a loop is detected, the graph optimization algorithm is used to optimize the existing map and pose trajectory to eliminate small drifts during the driving process. The graph optimization target is calculated using the sum of squared pose constraint deviations.

[0082]

[0083] In the formula, ξ represents the set of car poses. The feature point matching constraints are set for the i-th time and the j-th time. Finally, the laser point cloud data of all time are transformed into the UWB global coordinate system through real-time pose and rigid body transformation matrix T calculated in step 13. The data are then stitched together to form a complete global sand table map (the map resolution is 1cm, which meets the positioning accuracy requirements) and output to the car processing module in real time for subsequent data review and accuracy verification.

[0084] Step 3 is the UWB low-frequency verification stage. In this stage, UWB is only used as an external absolute coordinate reference source for periodic verification of SLAM output results, without providing feedback to correct SLAM pose.

[0085] The specific operation is as follows: The vehicle processing module presets the UWB wake-up interval (default 10s, which can be flexibly adjusted within the range of 5-15s according to accuracy requirements). At each preset interval, the processing module sends a wake-up command to wake up the dormant UWB positioning module. The duration of a single wake-up is ≤1s (to avoid long-term operation that consumes computing power). After the UWB positioning module is woken up, it quickly collects the absolute verification coordinates of the vehicle in the UWB global coordinate system. After data collection is complete, the system immediately enters sleep mode; simultaneously, the processing module extracts the current global coordinates of the vehicle calculated by the SLAM algorithm. (These coordinates have been automatically aligned to the UWB global coordinate system using the transformation matrix T, ensuring they are in the same coordinate system as the UWB verification coordinates and avoiding errors in deviation calculation); subsequently, the real-time positioning deviation is calculated using a formula. :

[0086]

[0087] Real-time deviation reflects the SLAM positioning accuracy at the current moment;

[0088] Simultaneously calculate the cumulative average deviation:

[0089]

[0090] In the formula, N represents the number of verifications, and the cumulative average deviation reflects the overall positioning accuracy of the algorithm; the processing module only records the real-time positioning deviation. Deviation from cumulative average The deviation data is not fed back to the SLAM algorithm for pose correction, but is only used for algorithm accuracy verification, review and subsequent optimization reference.

[0091] Step 4, End of Execution: This step is the final stage of the algorithm process. The core is to complete the algorithm execution judgment, data export, and process termination to ensure that the algorithm can run in a closed loop.

[0092] The specific operation is as follows: The vehicle processing module determines in real time whether the algorithm has completed. The determination conditions include two situations: one is that the preset running time has expired (e.g., preset running time of 30 minutes, which can be adjusted as needed), and the other is that a running end command is sent manually. If the running end condition is not met, the processing module controls the flow back to step 2, continues to execute SLAM autonomous closed-loop localization and mapping, and simultaneously executes the UWB low-frequency verification in step 3, forming a loop mode of "SLAM autonomous operation + UWB low-frequency verification". If the running end condition is met, the processing module controls the SLAM module and UWB module to stop working, ending the entire algorithm flow. At the same time, the processing module exports all core data, including SLAM initial pose data, dual coordinate system matching error data, real-time positioning deviation data, cumulative average deviation data, vehicle pose trajectory data, and global map data, for subsequent algorithm accuracy review, parameter optimization, and technical verification, ensuring that the algorithm performance is traceable and optimizable.

[0093] The above are merely preferred embodiments of the present invention and do not constitute any limitation on the scope of protection of the present invention; all technical solutions formed by equivalent transformations or equivalent substitutions fall within the scope of protection of the present invention; the parts of the present invention not described in detail are well-known technologies to those skilled in the art.

Claims

1. A UWB-based indoor SLAM initialization and localization method, characterized in that, The UWB-based indoor SLAM initialization localization method includes the following steps: Step 1: Initialize SLAM using UWB-driven SLAM, obtain the absolute initial pose of the vehicle in the UWB global coordinate system of the sand table, and achieve automatic matching between the UWB global coordinate system and the SLAM laser local coordinate system. Step 2: After initialization, the SLAM algorithm independently performs autonomous localization and global map construction within the sandbox based on the aligned global coordinate system. Step 3: The UWB positioning module is woken up at preset intervals to collect the current absolute verification coordinates and compare them with the current global coordinates of the vehicle calculated by the SLAM algorithm. The real-time positioning deviation and the cumulative average deviation are calculated. The deviation is only used for accuracy verification and is not fed back to the SLAM algorithm for real-time pose correction. Step 4: Determine if the conditions for ending the run are met. If not, repeat steps 2 to 3. If the conditions are met, end the run.

2. The UWB-based indoor SLAM initialization and positioning method according to claim 1, characterized in that, Step 1 also includes the following steps: Step 11: Activate the UWB positioning module and the laser scanning module. The UWB positioning module acquires the absolute initial coordinates of the vehicle body center in the UWB global coordinate system of the sand table in real time. Simultaneously, the initial heading angle between the car's driving direction and the positive X-axis direction of the UWB global coordinate system is collected. ; Step 12: The laser scanning module performs a comprehensive scan of the environment surrounding the sand table, identifying and acquiring the coordinates of three preset non-collinear reference feature points in the SLAM laser local coordinate system. ; Step 13: Automatic alignment of the two coordinate systems is completed using the rigid body transformation matrix T; Step 14: Calculate the average matching error. When e ≤ 5cm, the coordinate system is determined to be successfully matched, the UWB positioning module enters sleep mode, and SLAM initialization is completed. If e > 5cm, the laser feature points are re-acquired and calculated until the matching is successful.

3. The UWB-based indoor SLAM initialization and positioning method according to claim 2, characterized in that, In step 13, the rigid body transformation matrix is: T= ； in The initial heading angle acquired by UWB. The initial coordinates for UWB acquisition.

4. The UWB-based indoor SLAM initialization and positioning method according to claim 3, characterized in that, In step 13, the local coordinates of the three non-collinear reference feature points are substituted into the transformation formula to complete the alignment: ； The x-coordinate in the UWB global coordinate system The ordinate is the vertical coordinate in the UWB global coordinate system; Let x be the x-coordinate in the local coordinate system of SLAM laser. The vertical coordinate is the ordinate in the local coordinate system of SLAM laser. Solve its verification coordinates in the UWB global coordinate system. .

5. The UWB-based indoor SLAM initialization and positioning method according to claim 2, characterized in that, In step 14, the average matching error is: e= ； e represents the average matching error. Let i be the x-coordinate of the number i in the UWB global coordinate system. Let i be the ordinate of the number i in the UWB global coordinate system. Verify the x-coordinate of index i in the UWB global coordinate system. Verify the ordinate of point i in the UWB global coordinate system, where i is the number of the non-collinear reference feature point.

6. The UWB-based indoor SLAM initialization and positioning method according to claim 1, characterized in that, In step 2, the laser scanning module continuously collects point cloud data of the sand table environment in real time, and accurately extracts environmental feature points through the neighborhood point cloud curvature extraction algorithm. The curvature calculation adopts the neighborhood point distance deviation ratio method. By using the average distance from the neighborhood point to the target feature point, the sum of squares of the deviations of each neighborhood point from the average distance is calculated, and then divided by the product of the number of neighborhood points and the average distance to achieve accurate curvature calculation. Curvature calculation formula: ； Average distance formula: ； In the formula, Let i be the set of neighborhood points of the target feature point i. The number of neighboring points. Let j be the distance from the neighboring point j to the laser scanning module. The mean distance between neighboring points; curvature The larger the curvature, the more it tends towards the corner point; The smaller the value, the more it tends to be a planar point; Based on the initialized UWB global coordinate system, the real-time pose of the vehicle at time t is calculated using the pose update formula. The pose update formula is: ； in Δx and Δy represent the translational changes of the current time relative to the previous time, and Δθ represents the rotational changes. When the car travels to the scanned area in the sand table, loop closure detection is completed by feature point similarity matching. If a loop is detected, the graph optimization algorithm is used to optimize the existing map and pose trajectory to eliminate small drifts during the driving process. The graph optimization target is calculated by the sum of squared pose constraint deviations. ； In the formula, Let be the set of optimal poses after graph optimization. For the first The information matrix corresponding to the feature point matching constraints at time j and time j. Let ξ be the set of pose constraint edges, and ξ be the set of vehicle poses. The feature point matching constraint between time i and time j; Finally, the laser point cloud data at all times are transformed into the UWB global coordinate system through the real-time pose set ξ and the rigid body transformation matrix T calculated in step 13, and stitched together to form a complete global map of the sand table. The data is then output to the car processing module in real time for subsequent data review and accuracy verification.

7. The UWB-based indoor SLAM initialization and positioning method according to claim 1, characterized in that, In step 3, during the calculation of real-time positioning deviation, the UWB positioning module is woken up at preset time intervals, and the UWB positioning module collects the current absolute verification coordinates in the UWB global coordinate system. ; Simultaneously, extract the current global coordinates of the vehicle calculated by the SLAM algorithm. Real-time positioning deviation is calculated using a formula. : ； Real-time deviation reflects the SLAM positioning accuracy at the current moment; Simultaneously calculate the cumulative average deviation: ； In the formula, N represents the number of verifications and the cumulative average deviation. The overall positioning accuracy of the algorithm is reflected in its accuracy verification.

Images