Three-dimensional multi-robot positioning initialization method and system based on double UWB anchor points

Abstract

The invention discloses a three-dimensional multi-robot positioning initialization method and system based on double UWB anchor points, and belongs to the technical field of multi-robot cooperation. In order to solve the problem that multi-robot initialization depends on multi-anchor calibration and cross-machine communication in a GNSS missing environment, two static UWB anchor points which do not need to be accurately calibrated are deployed, a single robot independently completes sensor initialization and non-degeneration motion data acquisition, and the positions of the anchor points are solved through nonlinear optimization based on a UWB ranging sequence and an IMU gravity vector, so that the accuracy of multi-robot initialization is improved. And constructing a unique three-dimensional global coordinate system according to a right-hand rule, realizing the conversion from a local pose to a global pose, and ensuring the result reliability through quality self-evaluation. According to the method, cross-robot communication or centralized optimization is not needed, the deployment cost is low, the environmental adaptability is high, asynchronous online of multiple robots is supported, the initialization precision is high, the robustness is good, and the method is suitable for multi-robot collaborative operation in complex scenes such as tunnels, mines and warehouses.

Description

Technical Field

[0001] This invention belongs to the technical field, specifically relating to a three-dimensional multi-robot localization initialization method and system based on dual UWB (Ultra Wide Band) anchor points. Background Technology

[0002] In multi-robot collaborative operation scenarios of intelligent unmanned systems (such as underground inspection, warehousing and logistics, tunnel construction, and mine exploration), stable Global Navigation Satellite System (GNSS) signals are typically unavailable due to occlusion, enclosed structures, and material reflections. Each robot needs to achieve high-precision, time- and space-consistent positioning and map building in three-dimensional space to support functions such as path planning, cross-robot task allocation, global scheduling, and collaborative obstacle avoidance. Therefore, the system must complete the unified initialization of the global coordinate system at the beginning of the task—that is, aligning the local coordinate systems of each robot to the same three-dimensional global reference system. The lack of this unified benchmark will lead to: inconsistent map scales and origins across robots, fusion distortion, uncertain relative positional relationships, and difficulties in the stable execution of collaborative strategies (formation, coverage planning, and scheduling optimization). Existing multi-robot initialization and coordinate alignment schemes generally rely on external facilities or cross-robot information interaction. Common external auxiliary means include: (1) Radio frequency ranging or positioning infrastructure (UWB, Bluetooth, Wi-Fi): Deploy static anchor points with known absolute positions, and the robot calculates its own pose by measuring distance or angle; (2) Visual / laser markers (QR codes, reflectors, artificial landmarks): Identify the image or point cloud features of the markers and inversely determine the robot's pose in the global system.

[0003] In industrial settings (newly constructed tunnels, mine roadways, and densely racked warehouses), the deployment and maintenance of numerous precisely calibrated anchor points or markers are costly and inflexible. Environmental factors such as obstruction, multipath propagation, dust, and changes in lighting degrade the long-term stability of these facilities. If the initialization process relies on centralized or high-bandwidth communication, it introduces high memory consumption and computational / bandwidth pressure, making it difficult to meet the needs of scenarios with weak networks, asynchronous deployments, or limited edge computing resources. Furthermore, even after initial alignment, some systems still require continuous reliance on external anchor points or markers to suppress drift during operation, increasing energy consumption and introducing single-point-of-failure risks. Therefore, there is an urgent need for a multi-robot initialization method that features minimal infrastructure, requires no extensive precise prior knowledge, can operate in a decentralized manner, and can guarantee the uniqueness and stability of the coordinate system in three-dimensional space.

[0004] Existing solutions: 1. Initialization Scheme Based on Inter-Robot Communication and Mutual Observation: The core idea of ​​this method is to gradually establish cross-robot coordinate constraints and achieve global coordinate system alignment through relative observation and data exchange among multiple robots in a shared environment, without relying on any external anchor points or visual markers. The specific process typically includes: each robot autonomously exploring and running its own front-end odometry (vision, laser, wheeled, or inertial fusion); when mutual observation conditions occur (e.g., laser point cloud contour matching another robot, vision detecting the other robot's shape or marker, communication module acquiring the other robot's near-range signal features), relative pose estimation is triggered; subsequently, two or more robots exchange local map fragments, keyframe poses, feature descriptors, or loop closure candidate sets to construct cross-robot factors or constraints; then, distributed collaborative optimization or centralized graph optimization (e.g., multi-subgraph stitching + nonlinear least squares) is used to continuously converge to a unified global coordinate system. To enhance robustness, some implementations incorporate timestamp synchronization, loop closure verification, scale correction, and dynamic filtering strategies to reduce the impact of single-robot drift on overall consistency.

[0005] 2. Centralized Initialization Based on Three or More Known Coordinate UWB Anchor Points: This method pre-deploys at least three static UWB anchor points with precise 3D absolute coordinates in the environment. Before deployment, their positions need to be calibrated and recorded using a total station, laser rangefinder, or high-precision map. Upon robot startup, the distance sequence to multiple anchor points is measured simultaneously, and the robot's initial position and some attitude degrees of freedom are directly solved through trilateration or polygonal localization. In multi-robot scenarios, to ensure global consistency and suppress single-robot drift, the odometry data, anchor point distance residuals, and closure constraints of each robot's front end are typically constructed into a centralized factor graph, and then nonlinear optimization is performed to obtain the global solution.

[0006] 3. Coordinate Alignment Scheme Based on Visual / Laser Markers: This method involves installing visual or laser markers (such as QR codes, special reflective corner markers, laser corner reflectors, etc.) with identifiable codes and known geometric dimensions at several key locations in the environment. During the startup phase, the robot identifies the markers using a camera or laser sensor, calculates their pose relative to the marker's coordinate system, and then uses the pre-calibrated global coordinates of the markers for transformation to achieve initial global alignment. To enhance robustness, the system deploys multiple markers to provide redundancy and automatically selects the optimal marker for positioning within different viewpoints or distance ranges.

[0007] Disadvantages of existing technology: 1. Initialization scheme based on inter-robot communication and mutual observation: (1) Strong dependence on mutual vision and stable communication, delay in establishing cross-machine association under obstruction or weak network conditions, and uncontrollable initialization time; (2) Frequent exchange of keyframes, features or local maps is required, resulting in high bandwidth and synchronization requirements and large resource consumption; (3) When the initial observation distribution is singular, the heading or mirror degrees of freedom are easily left behind, and the uniqueness of the three-dimensional global coordinate system is insufficient; (4) Cross-robot odometry drift and scale error are amplified during alignment, affecting global consistency and accuracy.

[0008] 2. Centralized initialization based on three or more known coordinate UWB anchor points: (1) It relies on multiple anchor points for precise absolute calibration, resulting in high deployment and maintenance costs, and requires remeasurement for environmental adjustments; (2) In narrow or geometrically degenerate spaces, anchor points are nearly collinear, positioning conditions are degenerate, and initial solutions are unstable; (3) Centralized global optimization calculations consume a lot of memory, making it difficult to run efficiently on weak computing power or edge platforms, and it does not support large-scale robot parallel and asynchronous initialization; (4) The initialization and operation phases rely on continuous ranging and data aggregation from multiple anchor points, resulting in heavy communication and energy consumption burdens, and anchor point failures can lead to single-point risks.

[0009] 3. Coordinate alignment scheme based on visual / laser markers: (1) It is greatly affected by light, dust, smoke, obstruction and reflection interference, resulting in fluctuations in the stability of marker recognition and positioning accuracy; (2) High-density and reasonable deployment and periodic inspection are required. The deployment and maintenance costs are high, and the layout needs to be rearranged when the scene changes. (3) Recognition gaps are likely to occur at long distances, in extreme viewpoints or in narrow channels, and initialization is delayed and asynchronous online efficiency is low; (4) When the number of visible markers is insufficient in the early stage, attitude (especially heading and mirror image) disambiguation is insufficient, and short-term uniqueness is insufficient. Summary of the Invention

[0010] The technical problem to be solved by the present invention is to provide a three-dimensional multi-robot localization initialization method and system based on dual UWB anchor points, which addresses the shortcomings of the prior art. This method is used to solve the technical problem of how to achieve unique, low-drift, fast initialization and subsequent consistent alignment of the three-dimensional global coordinate system of a multi-robot system with the lowest external facilities and computing / bandwidth cost in environments with unknown absolute coordinates of anchor points, weak communication or occlusion.

[0011] The present invention adopts the following technical solution: A three-dimensional multi-robot localization initialization method based on dual UWB anchor points includes the following steps: S1. Deploy two static UWB anchor points A1 and A2 in the working environment and rigidly install the UWB tag, IMU and LiDAR or camera onto the robot body. S2. Collect static IMU data to estimate the gravity direction g, establish a ranging channel with the two static UWB anchor points A1 and A2, and start the odometer to output the relative pose. S3. The robot performs a motion involving changes in direction, and simultaneously collects N sets of observation data, including the distance from the robot to anchor point A1. Distance to anchor point A2 and the robot's pose in the local coordinate system L, output by the odometer. , ; S4. Based on the observation data, in the local coordinate system L, the position coordinates of the two static UWB anchor points A1 and A2 are solved using an optimization algorithm. and ; S5, using position coordinates Using the origin as the coordinate system, Pointing to position coordinates The vector and the direction of gravity g are constrained, and a unique three-dimensional global coordinate system G is constructed according to the right-hand coordinate system rule; S6. Transform the robot's initial pose in the local coordinate system L to the three-dimensional global coordinate system G constructed in step S5 through coordinate transformation to obtain the robot's initial pose in the three-dimensional global coordinate system G.

[0012] Preferably, in step S1, the two static UWB anchor points A1 and A2 do not need to be precisely calibrated in three-dimensional space. Their unique IDs are recorded during deployment, and fixed positions that are not easy to move and have a stable line of sight are selected. The two static UWB anchor points A1 and A2 are at the same horizontal height. After the UWB tag, IMU and lidar or camera are installed, the external parameters are fixed and the timestamp is unified to ensure that there is no relative displacement or angle drift during operation.

[0013] Preferably, in step S2, 2-3 seconds of stationary IMU data are collected, and the local gravity unit vector g is estimated by time averaging; the UWB tag establishes a two-way ranging channel with anchor points A1 and A2 to obtain real-time distance data; the odometer is a tightly coupled front end of laser and IMU or a tightly coupled front end of vision and IMU, and the UWB tag, IMU and odometer are all managed with a single unified time source for timestamp management.

[0014] Preferably, in step S3, the motion performed by the robot is a short-term non-degenerate autonomous motion involving a small amount of rotation; the observation data also includes the estimated gravity unit vector g from step S2, and all observation data are timestamped to form a complete observation sequence. The local coordinate system L takes the position of the robot at time i=1 as its origin.

[0015] Preferably, in step S4, the position coordinates of anchor points A1 and A2 are calculated. The specific process is as follows: For the collected The sequence is subjected to sliding window filtering; extraction The translation part in the middle, as the first Time Robot in Local Coordinate System The lower position ; Constructing nonlinear observation constraints: Establish a least squares optimization problem; The Levenberg-Marquardt algorithm is used to iteratively solve the least squares optimization problem, and the anchor points A1 and A2 in the local coordinate system are obtained. The position coordinates below .

[0016] Preferably, the problem is a nonlinear least squares problem:

[0017] in, This represents the number of time steps within the sampling window.

[0018] Preferably, in step S5, the specific process of constructing the three-dimensional global coordinate system G is as follows: Define the Z-axis direction of the three-dimensional global coordinate system G. ; Calculate anchor point baseline vector For the baseline vector Normalization is performed to obtain the initial X-axis unit vector. ; For the initial X-axis unit vector Perform Schmidt orthogonalization to remove the Z-axis component, and then normalize the processed vector to obtain the X-axis unit vector. ; Calculate the Y-axis unit vector according to the right-hand coordinate system rules. ; Construct rotation matrix Combined with the origin This forms a unique three-dimensional global coordinate system G.

[0019] Preferably, in step S6, the transformation matrix of the coordinate transformation is: The robot's initial pose in the local coordinate system L is a unit homogeneous transformation. The robot's initial pose in the three-dimensional global coordinate system G is: .

[0020] Preferably, after the robot reaches its initial pose in the three-dimensional global coordinate system G, a mass self-evaluation is initialized, specifically as follows: Calculate UWB ranging residuals , ;judge , , If the absolute values ​​are all lower than the preset angle tolerance, and all ranging residuals are less than the preset ranging threshold and the orthogonality of the coordinate axes meets the requirements, then the initialization is considered successful, the definition of the three-dimensional global coordinate system G and the robot's initial pose are published, and the robot is switched to the normal positioning mode. Subsequent positioning and mapping will no longer rely on UWB ranging. If the conditions are not met, the robot will return to step S3 and re-execute the motion and data acquisition.

[0021] Preferably, multiple robots execute steps S1 to S6 independently and in parallel. Each robot constructs a three-dimensional global coordinate system G according to the unified rules of step S5, realizing direct stitching and collaborative planning of local maps of multiple robots without the need for cross-robot communication or centralized optimization.

[0022] Secondly, embodiments of the present invention provide a three-dimensional multi-robot localization initialization system based on dual UWB anchor points, comprising: Anchor point module is used to deploy two static UWB anchor points A1 and A2 in the working environment, rigidly install UWB tags, IMU and LiDAR or camera to the robot body and complete the fixation of external parameters and the unification of timestamps; The initialization module is used to acquire static IMU data to estimate the gravity direction g, establish a ranging channel with the two static UWB anchor points A1 and A2, and start the odometer to output the relative pose. The data acquisition module is used to control the robot to perform movements involving changes in direction and to simultaneously acquire N sets of observation data, including the distance from the robot to anchor point A1. Distance to anchor point A2 and the robot's pose in the local coordinate system L, output by the odometer. , ; The solution module is used to solve for the position coordinates of the two static UWB anchor points A1 and A2 in the local coordinate system L based on the observation data and using an optimization algorithm. and ; Build modules for using position coordinates Using the origin as the coordinate system, Pointing to position coordinates The vector and the direction of gravity g are constrained, and a unique three-dimensional global coordinate system G is constructed according to the right-hand coordinate system rule; The calculation module is used to transform the robot's initial pose in the local coordinate system L to the three-dimensional global coordinate system G through coordinate transformation, so as to obtain the robot's initial pose in the three-dimensional global coordinate system G.

[0023] Preferably, the global coordinate system construction module includes an orthogonalization processing unit for processing the initial X-axis unit vector. Perform Schmidt orthogonalization to remove the Z-axis component and ensure the orthogonality of the coordinate axes of the three-dimensional global coordinate system G.

[0024] Preferably, the observation data acquisition module is also used to control the robot to perform short-term non-degenerate autonomous motion involving a small amount of rotation, and to timestamp the acquired observation data to form a complete observation sequence containing the gravity unit vector g.

[0025] Preferably, the anchor point location solution module includes a data preprocessing unit and an optimization solution unit; the data preprocessing unit is used to perform sliding window filtering on the UWB ranging data and remove outliers; the optimization solution unit is used to construct a nonlinear least squares model and use the Levenberg-Marquardt algorithm to iteratively solve for the anchor point location coordinates. and .

[0026] Preferably, it also includes a quality assessment and feedback module, which is used to calculate the UWB ranging residual and the orthogonality of the coordinate axes, and determine whether the initialization is successful based on the preset ranging threshold and angle tolerance. If successful, it triggers the global coordinate system and initial pose to be published and switches to the normal positioning mode. If it fails, it triggers the observation data acquisition module to re-execute data acquisition.

[0027] Thirdly, a computer device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the above-described three-dimensional multi-robot localization initialization method based on dual UWB anchor points.

[0028] Fourthly, embodiments of the present invention provide a computer-readable storage medium including a computer program, which, when executed by a processor, implements the steps of the above-described three-dimensional multi-robot localization initialization method based on dual UWB anchor points.

[0029] Fifthly, a chip includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the above-described three-dimensional multi-robot localization initialization method based on dual UWB anchor points.

[0030] In a sixth aspect, embodiments of the present invention provide an electronic device, including a computer program, which, when executed by the electronic device, implements the steps of the above-described three-dimensional multi-robot localization initialization method based on dual UWB anchor points.

[0031] Compared with the prior art, the present invention has at least the following beneficial effects: A 3D multi-robot localization initialization method based on dual UWB anchor points addresses the challenge of rapidly, uniquely, and robustly establishing a unified global spatial reference for multiple robots in underground, enclosed, obstructed, and weakly networked industrial environments (such as tunnels, mines, underground parking garages, densely packed warehouse passages, and complex factory workstations) with no GNSS signal or extremely poor GNSS quality. The core technical means of this invention include: minimal facility deployment (only two static UWB anchor points that do not require absolute calibration), constructing a unique right-handed 3D coordinate system using the anchor point baseline and IMU gravity vector, and result publishing and subsequent multi-robot parallel reuse. The entire method is executed independently by a single robot, eliminating the need for cross-robot communication, centralized graph optimization, network-wide time synchronization, or precise multi-anchor point calibration. It supports asynchronous parallel initialization for multiple robots while ensuring coordinate system consistency.

[0032] Furthermore, the stipulation that anchor points do not require precise calibration of their three-dimensional spatial coordinates and must be at the same horizontal level significantly reduces the requirements for on-site construction personnel, allowing for placement without specialized surveying equipment, thus enhancing the method's practicality and scalability. The requirement that extrinsic parameters be fixed and timestamps unified after sensor installation is a technical prerequisite for achieving high-precision fusion of subsequent multi-sensor data. This constraint ensures the spatiotemporal consistency of data observed by different sensors when the robot moves as a rigid body, thereby guaranteeing the accuracy of optimizing anchor point positions in step S4 and the effectiveness of the gravity direction and baseline vector constraints in step S5. This is a crucial foundation for the stable operation of the entire method.

[0033] Furthermore, a standardized startup process ensures the quality and reliability of sensor data. Acquiring stationary IMU data to estimate the direction of gravity is key to constraining the vertical direction using the Earth's gravitational field as an absolute physical reference; this method is simple and robust. Establishing a two-way ranging (TWR) channel with the anchor point effectively offsets clock drift compared to unidirectional ranging, improving the accuracy of UWB distance measurement. Designating the odometer as the tightly coupled front end of the laser / vision system and the IMU indicates the preferred technical path for achieving high-precision local relative pose estimation. Tight-coupled fusion fully utilizes the high-frequency dynamic characteristics of the IMU and the absolute observation of the vision / laser system, suppressing odometer drift. All modules use a unified time source for timestamp management, a core technical measure to ensure synchronous fusion processing of asynchronous data streams, avoiding system errors introduced by time asynchrony.

[0034] Furthermore, the minimum sufficient conditions required for successful initialization are defined. Short-duration non-degenerate autonomous motion means that the robot only needs to perform a simple movement involving posture changes (such as turning), without complex trajectories, which shortens the initialization preparation time and ensures a good condition number for the observation matrix, making the optimization problem in step S4 solvable. Incorporating the gravity vector g into the observation sequence clarifies the role of this key physical quantity in the entire data stream. The local coordinate system L is defined with the robot's position at time i=1 as the origin. This is a concise and reasonable coordinate system definition method, providing a clear and unified reference framework for all subsequent calculations, simplifying mathematical modeling and implementation.

[0035] Furthermore, the problem is transformed into a well-defined, numerically solvable optimization problem with a solid mathematical foundation. By applying sliding window filtering to the UWB ranging data, noise can be smoothed and outliers caused by multipath effects can be removed, improving the quality of the input data. The odometry translation position is extracted and correlated with the ranging value using the Euclidean distance formula, constructing an intuitive geometric constraint. A nonlinear least squares optimization problem is established and solved using the Levenberg-Marquardt algorithm, a mature, efficient, and robust standard method for solving such geometric localization problems. This claim clearly demonstrates how to stably estimate the relative positions of two anchor points in the robot's own coordinate system from raw, noisy observation data; this is a core step in data preparation for constructing a global coordinate system.

[0036] Furthermore, by quantifying the sum of squared ranging errors as the optimization objective, the anchor point position calculation has a clear mathematical basis, avoiding subjective empirical design. Simultaneously, the distance relationship between the two anchor points and the robot is constrained, utilizing the redundancy of multiple sets of observation data to suppress the impact of errors in a single set of data, thus improving the robustness of the solution. Compared to the fuzzy optimization objective design of existing schemes, this model makes the solution process quantifiable and reproducible, facilitating engineering implementation and parameter tuning. At the same time, by summing and accumulating data, it fully utilizes the information from N sets of observation data, further improving the accuracy of anchor point position estimation and laying a solid foundation for the construction of a global coordinate system.

[0037] Furthermore, with only two anchor points, the degrees of freedom for rotation around the baseline (heading angle) and mirror ambiguity remain in three-dimensional space. By introducing the absolute gravity direction provided by the IMU (defining the Z-axis) and forcing the use of the right-hand rule (calculating the Y-axis), the orientations of all three coordinate axes are fixed at once. The "Schmidt orthogonalization" step is crucial, ensuring that even if the line connecting the two anchor points is not perfectly horizontal, a strictly orthogonal X-axis to the gravity direction (Z-axis) can be obtained through projection, thus constructing a standard, orthogonal right-handed Cartesian coordinate system. This process is deterministic, provided the inputs are the same ( , g), output ( This ensures that a completely consistent global reference system can be generated when multiple robots operate independently.

[0038] Furthermore, the initialization process is completed, and the results of all the preceding steps are integrated into a concise mathematical transformation. Transformation matrix. Its structure is intuitive and clear: its rotating part This represents the global coordinate system pose constructed in step S5, with its translation component p1 representing the origin of the global coordinate system (i.e., the position of anchor point A1). Since the robot's initial pose in the local coordinate system L is defined as a "unit transformation," its initial pose in the global coordinate system G is this transformation matrix itself. This claim clearly defines the robot's state upon "initialization completion," namely its initial position and orientation in the newly constructed global coordinate system, providing precise initial conditions for subsequent localization, mapping, and navigation.

[0039] Furthermore, by verifying the results using both ranging residuals and coordinate axis orthogonality, unqualified initialization results can be effectively screened, preventing errors from being propagated to subsequent operation stages. The threshold judgment mechanism enables automated decision-making; if successful, it switches to the conventional positioning mode and no longer relies on UWB ranging, reducing energy consumption and single-point failure risk; if unsuccessful, it rolls back and resamples, forming a closed-loop optimization. The core advantage lies in improving the reliability and engineering practicality of the initialization results, solving the problems of lack of quality verification and low result credibility in existing solutions; the self-evaluation and rollback mechanisms require no manual intervention, adapting to unmanned operation scenarios, and the operation stage is free from UWB dependence, further enhancing the system's robustness and endurance.

[0040] Furthermore, each robot independently completes its initialization process, constructing a global coordinate system according to unified rules to achieve decentralized collaboration. The advantages include eliminating the need for cross-robot communication or centralized optimization, avoiding bandwidth consumption and synchronization issues, and adapting to large-scale robot clusters. Unified system construction rules ensure that all robots have naturally consistent global coordinate systems, allowing for direct stitching of local maps. Collaborative planning requires no additional scale or orientation correction, solving the problems of complex multi-robot alignment and high synchronization costs in existing solutions. Asynchronous deployment support enables robots to be started in batches, improving deployment flexibility and significantly reducing the initialization difficulty and collaboration costs of large-scale multi-robot systems.

[0041] It is understood that the beneficial effects of the second to sixth aspects mentioned above can be found in the relevant descriptions in the first aspect mentioned above, and will not be repeated here.

[0042] In summary, this invention achieves the unique and rapid construction of a multi-robot 3D global coordinate system using only two uncalibrated UWB anchor points, combined with IMU gravity vectors and odometry. It features extremely simple facilities, significantly reducing deployment and maintenance costs; decentralization, with each robot operating independently without inter-robot communication, adapting to weak network environments; unique determination, completely eliminating ambiguity between heading and mirroring; a closed-loop quality system with self-evaluation and rollback mechanisms, ensuring strong robustness; and inherent consistency, supporting asynchronous parallel initialization across multiple robots with automatic coordinate system alignment.

[0043] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0044] Figure 1 This is a flowchart of the method of the present invention; Figure 2 This is a schematic diagram of dual-anchor-point ranging data acquisition at time i; Figure 3 A schematic diagram of a computer device provided in an embodiment of the present invention; Figure 4 This is a block diagram of a chip provided according to an embodiment of the present invention.

[0045] Among them, 60. Computer equipment; 61. Processor; 62. Memory; 63. Computer program; 600. Electronic device; 610. Processing unit; 620. Storage unit; 6201. Random access memory unit; 6202. Cache memory unit; 6203. Read-only memory unit; 6204. Program / utility; 6205. Program module; 630. Bus; 640. Display unit; 650. Input / output interface; 660. Network adapter; 700. External device. Detailed Implementation

[0046] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0047] In the description of this invention, it should be understood that the terms "comprising" and "including" indicate the presence of the described features, integrals, steps, operations, elements and / or components, but do not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components and / or collections thereof.

[0048] It should also be understood that the terminology used in this specification is for the purpose of describing particular embodiments only and is not intended to limit the invention. As used in this specification and the appended claims, the singular forms “a,” “an,” and “the” are intended to include the plural forms unless the context clearly indicates otherwise.

[0049] It should also be further understood that the term "and / or" as used in this specification and the appended claims refers to any combination and all possible combinations of one or more of the associated listed items, and includes such combinations. For example, A and / or B can represent three cases: A alone, A and B simultaneously, and B alone. Additionally, the character " / " in this invention generally indicates that the preceding and following objects have an "or" relationship.

[0050] It should be understood that although terms such as first, second, third, etc., may be used in the embodiments of the present invention to describe the preset range, these preset ranges should not be limited to these terms. These terms are only used to distinguish the preset ranges from one another. For example, without departing from the scope of the embodiments of the present invention, the first preset range may also be referred to as the second preset range, and similarly, the second preset range may also be referred to as the first preset range.

[0051] Depending on the context, the word "if" as used here can be interpreted as "when," "when," "in response to determination," or "in response to detection." Similarly, depending on the context, the phrase "if determination" or "if detection (of the stated condition or event)" can be interpreted as "when determination," "in response to determination," "when detection (of the stated condition or event)," or "in response to detection (of the stated condition or event)."

[0052] The accompanying drawings illustrate various structural schematic diagrams according to embodiments disclosed in this invention. These drawings are not to scale, and some details have been enlarged for clarity, and some details may have been omitted. The shapes of the various regions and layers shown in the drawings, as well as their relative sizes and positional relationships, are merely exemplary and may deviate from reality due to manufacturing tolerances or technical limitations. Furthermore, those skilled in the art can design regions / layers with different shapes, sizes, and relative positions as needed.

[0053] This invention provides a 3D multi-robot localization initialization method based on dual UWB anchor points, applicable to GNSS-deficient environments such as tunnels, mines, underground construction sites, and warehouses. It requires only the deployment of two static UWB anchor points of the same height but unknown location, eliminating the need for precise calibration, visual / laser markers, cross-robot communication, or network-wide time synchronization, and does not rely on centralized global graph optimization. Each robot acquires a dual-anchor-point ranging sequence and an inertial measurement unit (IMU) gravity vector through short-term non-degenerate autonomous motion, performing robust small-scale optimization locally to directly construct a unique right-handed 3D coordinate system and obtain an initial pose compatible with the cluster. This method significantly reduces the deployment and maintenance costs of multiple anchor points and markers, avoids mutual gaze and high-bandwidth exchange, eliminates heading and mirror ambiguity, supports weak networks and asynchronous deployment, has low computational and memory consumption, and no longer continuously relies on ranging after initialization, reducing energy consumption and single-point failure risks, and improving the deployment efficiency and global consistency of multiple robots in complex occlusion and narrow spaces.

[0054] Please see Figure 1 The present invention provides a three-dimensional multi-robot localization initialization method based on dual UWB anchor points, comprising the following steps: S1. Equipment Preparation Rigidly mount the UWB, IMU, and LiDAR or camera onto the robot body, ensuring unified timestamps and fixed extrinsic parameters. Deploy two static UWB anchor points, A1 and A2, in the working environment. The anchor points do not need to be precisely calibrated; they only need to be approximately at the same horizontal level.

[0055] Three UWB modules (one for airborne UWB tags and two for static UWB anchors); one airborne terminal; one LiDAR (Light Detection and Ranging) or camera for acquiring front-end odometry data; and one IMU for acquiring inertial data and gravity direction.

[0056] A UWB tag, IMU, and LiDAR or camera are rigidly fixed to the robot body to solidify extrinsic parameters, ensuring that the three components satisfy rigid body constraints and have no relative displacement or angular drift during operation. Two static UWB anchors, designated A1 and A2, are deployed in the target working area. Their unique IDs are recorded during deployment, and fixed locations with stable line-of-sight and difficult-to-move positions are selected. These two anchors do not require pre-measurement of their 3D spatial coordinates, nor do they require high-precision installation or full-network clock synchronization; they only need to be approximately at the same horizontal level to ensure that the subsequent connection between the gravity vector and the anchor points constitutes an effective 3D constraint.

[0057] S2, Sensor Initialization After the robot is powered on, it collects 2-3 seconds of stationary IMU data to estimate the direction and bias of gravity; it starts the UWB module to identify the anchor point ID and establish a ranging channel; and it starts the odometry front end (laser or vision) to output the relative pose.

[0058] Please see Figure 2 After the robot is powered on, it performs local sensor initialization without relying on external communication. The IMU module is activated, and with the robot stationary, the IMU output is time-averaged to estimate the local gravity vector direction (unit vector g), which serves as the absolute vertical reference. The UWB communication module is activated to scan and identify static UWB anchor points in the environment and receive their broadcast IDs; two-way ranging is established with A1 and A2. The Way Ranging (TWR) channel acquires real-time distance data for each anchor point. The tightly coupled front-end odometry of the laser or vision system and IMU is activated, the local state estimator is initialized, and the motion trajectory and pose sequence are recorded in its own coordinate system. All three sub-modules use a single-machine unified time source for timestamp management to ensure temporal consistency in subsequent data fusion and coordinate system construction.

[0059] S3, Motivation and Data Acquisition The robot performs non-degenerate motion excitations for a period of time, including changes in direction or a small amount of rotation, and simultaneously collects N sets of UWB ranging values ​​(distances from each robot to A1 and A2) and local odometry pose sequences.

[0060] The robot performs a short-duration non-degenerate autonomous motion involving changes in direction or a small amount of rotation. Within a sampling window, N sets of multi-source sensor data are simultaneously acquired (N is the number of time steps within the sampling window, i=1, ..., N). To ensure consistency in subsequent modeling, a local initial coordinate system L is established at time i=1. The i-th set of data is defined as follows: UWB ranging value: This represents the distance to anchor point A1. This indicates the distance to anchor point A2; Gravity direction: use the unit gravity vector g estimated in step S2; Local pose of the odometer output : Represents the position and orientation of the robot in the local initial coordinate system L at time i; All data are timestamped using a single, unified time source to form an observation sequence. And it is matched with the direction of gravity g.

[0061] S4. Anchor point position estimation in local coordinate system The acquired UWB ranging sequence is filtered by sliding window and outliers are removed. A nonlinear observation model is constructed together with the local odometry pose. The three-dimensional coordinates of A1 and A2 in the robot's local coordinate system are solved by least squares optimization.

[0062] Based on the observation sequence in step S3, the three-dimensional coordinates of the two UWB anchor points in the robot's local initial coordinate system L (with the robot's position at time i=1 as the origin) are solved.

[0063] First, the acquired UWB ranging sequence is subjected to sliding window filtering and outliers are removed; Let the unknown positions of A1 and A2 in L be respectively ; The robot's position at time i is denoted as . (Taken from (the translational portion), then the theoretical distance measurement should satisfy:

[0064] Where i = 1, ..., N.

[0065] Constructing a nonlinear least squares problem:

[0066] The Levenberg-Marquardt algorithm is used for iterative solution. , obtained The relative position of the anchor point is used as the input for constructing a unique global coordinate system in subsequent steps.

[0067] S5. Construct a unique global coordinate system Using the optimized position A1 as the origin, the direction A1→A2 is normalized to the X-axis, and the opposite direction of IMU gravity is the Z-axis. After orthogonalizing and adjusting X and Z, Y=Z×X is calculated to form a unique three-dimensional global coordinate system.

[0068] To eliminate redundancy in the degrees of freedom of the 3D reference derived solely from the two UWB anchor points in terms of heading, rotation, and mirroring, this step explicitly generates a unique global coordinate system according to the "anchor point baseline + gravity vector + right-hand rule": (1) The result obtained in step S4 As the origin O of the global coordinate system; (2) Take the unit gravity vector g obtained in step S2, and define the Z-axis direction as the opposite direction of gravity. ; (3) Calculate the anchor point baseline vector Normalization yields the initial X-axis unit vector. ; (4) Schmidt orthogonalization, used to remove the x-component in z: Normalization yields ; (5) Calculate the Y-axis direction according to the right-hand coordinate system convention: ; (6) Construct the rotation matrix , where the column vectors correspond to the X, Y, and Z axes, respectively.

[0069] By using the above constraints, the uncertainties of arbitrary rotation around the baseline and mirroring are eliminated, resulting in a single and reproducible three-dimensional global reference frame.

[0070] S6, Calculate the robot's global initial pose. The robot's initial pose in the local coordinate system is transformed into the global coordinate system constructed in step S5 through coordinate transformation, so as to obtain its initial position and attitude in the unified three-dimensional global coordinate system.

[0071] After constructing the global coordinate system, determine the robot's initial pose in that coordinate system. The robot's initial pose in the local coordinate system is determined by a unit transformation. This refers to a homogeneous transformation of units.

[0072] Based on the result of step S5, the transformation of the global coordinate system relative to the local coordinate system is as follows: Therefore, the robot's initial pose in the global coordinate system is: Multiple robots independently generate their own data according to the same rules. and Because the coordinate system is consistent and there are no multiple solutions or ambiguities, the initial pose and the subsequent map are naturally in the same global reference system, and can be directly aligned and stitched together.

[0073] S7. Initial Quality Self-Assessment Calculate the orthogonality between the UWB ranging residual and the coordinate axis. If all of them meet the preset threshold, the result is successful. Publish the global coordinate system definition and switch to the normal positioning mode. Do not continuously rely on UWB ranging, and the global coordinate system remains stable. If the condition is not met, return to step S3 and repeat motion excitation and data acquisition.

[0074] To improve engineering reliability, the system automatically evaluates the quality of the initialization results and calculates the orthogonality between the UWB ranging residual and the coordinate axes. (1) Distance residual: Calculate the distance fitting deviation at each time step. ; (2) Orthogonality of coordinate axes: Check , , Is the absolute value lower than the angle tolerance?

[0075] If all indicators pass, initialization is successful: Publish global coordinate system definition (origin O, rotation matrix) The anchor point relative position and the robot's initial pose are determined; once the system enters normal operation, all subsequent positioning, mapping, and navigation are completed based on the onboard sensors, and UWB ranging is no longer continuously called.

[0076] If any indicator fails (such as excessive distance residual or non-orthogonal coordinate axes), the result is considered unreliable: the system reverts to step S3, prompts for re-excitation of motion, and repeats steps S4 to S7 after data re-acquisition.

[0077] This self-assessment closed loop ensures that the published global coordinate system is unique, stable, and reproducible, and prevents unqualified initial results from being propagated to subsequent operation phases.

[0078] The process of this invention supports multiple robots to execute independently and in parallel. Each robot generates a completely consistent global 3D reference system according to the same rules, thereby realizing direct map stitching and collaborative planning.

[0079] In another embodiment of the present invention, a three-dimensional multi-robot localization initialization system based on dual UWB anchor points is provided. This system can be used to implement the above-mentioned three-dimensional multi-robot localization initialization method based on dual UWB anchor points. Specifically, the three-dimensional multi-robot localization initialization system based on dual UWB anchor points includes a module, an initialization module, an acquisition module, a solution module, a construction module, and a calculation module.

[0080] Among them, the anchor point module is used to deploy two static UWB anchor points A1 and A2 in the working environment, rigidly install the UWB tag, IMU and LiDAR or camera on the robot body and complete the fixation of external parameters and the unification of timestamps; The initialization module is used to acquire static IMU data to estimate the gravity direction g, establish a ranging channel with the two static UWB anchor points A1 and A2, and start the odometer to output the relative pose. The data acquisition module is used to control the robot to perform movements involving changes in direction and to simultaneously acquire N sets of observation data, including the distance from the robot to anchor point A1. Distance to anchor point A2 and the robot's pose in the local coordinate system L, output by the odometer. , ; The solution module is used to solve for the position coordinates of the two static UWB anchor points A1 and A2 in the local coordinate system L based on the observation data and using an optimization algorithm. and ; Build modules for using position coordinates Using the origin as the coordinate system, Pointing to position coordinates The vector and the direction of gravity g are constrained, and a unique three-dimensional global coordinate system G is constructed according to the right-hand coordinate system rule; The calculation module is used to transform the robot's initial pose in the local coordinate system L to the three-dimensional global coordinate system G through coordinate transformation, so as to obtain the robot's initial pose in the three-dimensional global coordinate system G.

[0081] The global coordinate system construction module includes an orthogonalization processing unit, used to process the initial X-axis unit vector. Perform Schmidt orthogonalization to remove the Z-axis component and ensure the orthogonality of the coordinate axes of the three-dimensional global coordinate system G.

[0082] The observation data acquisition module is also used to control the robot to perform short-term non-degenerate autonomous motion involving a small amount of rotation, and to timestamp the acquired observation data to form a complete observation sequence containing the gravity unit vector g.

[0083] The anchor point location solution module includes a data preprocessing unit and an optimization solution unit. The data preprocessing unit performs sliding window filtering on the UWB ranging data and removes outliers. The optimization solution unit constructs a nonlinear least squares model and uses the Levenberg-Marquardt algorithm to iteratively solve for the anchor point location coordinates. and .

[0084] It also includes a quality assessment and feedback module, which is used to calculate the UWB ranging residual and the orthogonality of the coordinate axes. It determines whether the initialization is successful based on the preset ranging threshold and angle tolerance. If successful, it triggers the global coordinate system and initial pose to be published and switches to the normal positioning mode. If it fails, it triggers the observation data acquisition module to re-execute data acquisition.

[0085] This invention provides a terminal device comprising a processor and a memory. The memory stores a computer program, which includes program instructions. The processor executes the program instructions stored in the computer storage medium. The processor may be a Central Processing Unit (CPU), or other general-purpose processors, graphics processing units (GPUs), tensor processing units (TPUs), digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. It is the computing and control core of the terminal, suitable for implementing one or more instructions, specifically suitable for loading and executing one or more instructions to achieve a corresponding method flow or function. The processor described in this embodiment can be used for the operation of a three-dimensional multi-robot localization initialization method based on dual UWB anchor points, including: Two static UWB anchor points A1 and A2 are deployed in the working environment. UWB tags, an IMU, and a LiDAR or camera are rigidly mounted to the robot body. Static IMU data is collected to estimate the gravity direction g, and a ranging channel is established with the two static UWB anchor points A1 and A2. The odometry is activated to output the relative pose. The robot performs motions involving changes in direction, and N sets of observation data are collected simultaneously. These observation data include the distance from the robot to anchor point A1. Distance to anchor point A2 and the robot's pose in the local coordinate system L, output by the odometer. , Based on the observed data, in the local coordinate system L, the position coordinates of the two static UWB anchor points A1 and A2 are solved using an optimization algorithm. and ; using position coordinates Using the origin as the coordinate system, Pointing to position coordinates The vector is constrained by the direction of gravity g. According to the right-hand coordinate system rule, a unique three-dimensional global coordinate system G is constructed. The initial pose of the robot in the local coordinate system L is transformed to the constructed three-dimensional global coordinate system G through coordinate transformation, so as to obtain the initial pose of the robot in the three-dimensional global coordinate system G.

[0086] Please see Figure 3 The terminal device is a computer device. In this embodiment, the computer device 60 includes a processor 61, a memory 62, and a computer program 63 stored in the memory 62 and executable on the processor 61. When executed by the processor 61, the computer program 63 implements the 3D multi-robot localization initialization method based on dual UWB anchor points described in this embodiment. To avoid repetition, details are omitted here. Alternatively, when executed by the processor 61, the computer program 63 implements the functions of each model / unit in the 3D multi-robot localization initialization system based on dual UWB anchor points described in this embodiment. To avoid repetition, details are omitted here.

[0087] Computer device 60 can be a desktop computer, laptop, handheld computer, cloud server, or other computing device. Computer device 60 may include, but is not limited to, a processor 61 and a memory 62. Those skilled in the art will understand that... Figure 3 This is merely an example of computer device 60 and does not constitute a limitation on computer device 60. It may include more or fewer components than shown, or combine certain components, or different components. For example, computer device may also include input / output devices, network access devices, buses, etc.

[0088] The processor 61 may be a Central Processing Unit (CPU), or other general-purpose processors, graphics processing units (GPUs), tensor processing units (TPUs), digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor.

[0089] The memory 62 can be an internal storage unit of the computer device 60, such as a hard disk or RAM of the computer device 60. The memory 62 can also be an external storage device of the computer device 60, such as a plug-in hard disk, smart media card (SMC), secure digital (SD) card, flash card, etc., provided on the computer device 60.

[0090] Furthermore, the memory 62 may include both internal storage units of the computer device 60 and external storage devices. The memory 62 is used to store computer programs and other programs and data required by the computer device. The memory 62 can also be used to temporarily store data that has been output or will be output.

[0091] Please see Figure 4 The terminal device is an electronic device 600, which is manifested in the form of a general-purpose computing device. The components of the electronic device may include, but are not limited to: at least one processing unit 610, at least one storage unit 620, a bus 630 connecting different platform components (including storage unit 620 and processing unit 610), a display unit 640, etc.

[0092] The storage unit stores program code, which can be executed by the processing unit 610 to perform the steps described in the method section of this specification according to various exemplary embodiments of the present invention. For example, the processing unit 610 can perform actions such as... Figure 1 The steps are shown in the figure.

[0093] Storage unit 620 may include a readable medium in the form of a volatile storage unit, such as random access memory (RAM) 6201 and / or cache memory 6202, and may further include a read-only memory (ROM) 6203.

[0094] Storage unit 620 may also include a program / utility 6204 having a set (at least one) program module 6205, such program module 6205 including but not limited to: operating system, one or more application programs, other program modules and program data, each or some combination of these examples may include an implementation of a network environment.

[0095] Bus 630 can represent one or more of several types of bus structures, including a memory cell bus or memory cell controller, a peripheral bus, a graphics acceleration port, a processing unit, or a local bus using any of the multiple bus structures.

[0096] Electronic device 600 can also communicate with one or more external devices 700 (e.g., keyboard, pointing device, Bluetooth device, etc.), and with one or more devices that enable a user to interact with electronic device 600, and / or with any device that enables electronic device 600 to communicate with one or more other computing devices (e.g., router, modem). This communication can be performed via input / output interface 650. Furthermore, electronic device 600 can also communicate with one or more networks (e.g., local area network, wide area network, and / or public network, such as the Internet) via network adapter 660. Network adapter 660 can communicate with other modules of electronic device 600 via bus 630. It should be understood that, although not shown in the figures, other hardware and / or software modules can be used in conjunction with electronic device 600, including but not limited to: microcode, device drivers, redundant processing units, external disk drive arrays, RAID systems, tape drives, and data backup storage platforms.

[0097] Example 4 This invention also provides a storage medium, specifically a computer-readable storage medium, which is a memory device in a terminal device for storing programs and data. It is understood that the computer-readable storage medium here can include both built-in storage media in the terminal device and extended storage media supported by the terminal device; it can be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, apparatus, or device. The computer-readable storage medium provides storage space that stores the terminal's operating system. Furthermore, the storage space also stores one or more instructions suitable for loading and execution by a processor, which can be one or more computer programs (including program code). More specific examples of the computer-readable storage medium include: an electrical connection with one or more wires, a portable disk, a hard disk, random access memory, read-only memory, erasable programmable read-only memory, optical fiber, portable compact disk read-only memory, optical storage device, magnetic storage device, or any suitable combination thereof.

[0098] Computer-readable storage media also include data signals propagated in baseband or as part of a carrier wave, carrying readable program code. Such propagated data signals can take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. A readable storage medium can also be any readable medium other than a readable storage medium that can send, propagate, or transmit a program for use by or in connection with an instruction execution system, apparatus, or device. The program code contained on the readable storage medium can be transmitted using any suitable medium, including but not limited to wireless, wired, optical fiber, radio frequency, etc., or any suitable combination thereof.

[0099] Program code for performing the operations of this invention can be written in any combination of one or more programming languages, including object-oriented programming languages ​​such as Java and C++, and conventional procedural programming languages ​​such as C or similar languages. The program code can execute entirely on the user's computing device, partially on the user's computing device, as a standalone software package, partially on the user's computing device and partially on a remote computing device, or entirely on a remote computing device or server. In cases involving remote computing devices, the remote computing device can be connected to the user's computing device via any type of network, including a local area network (LAN) or a wide area network (WAN), or it can be connected to an external computing device (e.g., via the Internet using an Internet service provider).

[0100] One or more instructions stored in a computer-readable storage medium can be loaded and executed by a processor to implement the corresponding steps of the three-dimensional multi-robot localization initialization method based on dual UWB anchor points in the above embodiments; one or more instructions in the computer-readable storage medium are loaded and executed by the processor in the following steps: Two static UWB anchor points A1 and A2 are deployed in the working environment. UWB tags, an IMU, and a LiDAR or camera are rigidly mounted to the robot body. Static IMU data is collected to estimate the gravity direction g, and a ranging channel is established with the two static UWB anchor points A1 and A2. The odometry is activated to output the relative pose. The robot performs motions involving changes in direction, and N sets of observation data are collected simultaneously. These observation data include the distance from the robot to anchor point A1. Distance to anchor point A2 and the robot's pose in the local coordinate system L, output by the odometer. , Based on the observed data, in the local coordinate system L, the position coordinates of the two static UWB anchor points A1 and A2 are solved using an optimization algorithm. and ; using position coordinates Using the origin as the coordinate system, Pointing to position coordinates The vector is constrained by the direction of gravity g. According to the right-hand coordinate system rule, a unique three-dimensional global coordinate system G is constructed. The initial pose of the robot in the local coordinate system L is transformed to the constructed three-dimensional global coordinate system G through coordinate transformation, so as to obtain the initial pose of the robot in the three-dimensional global coordinate system G.

[0101] The databases involved in the embodiments provided in this application may include at least one type of relational database and non-relational database. Non-relational databases may include, but are not limited to, blockchain-based distributed databases. The processors involved in the embodiments provided in this application may be general-purpose processors, central processing units, graphics processing units, digital signal processors, programmable logic devices, quantum computing-based data processing logic devices, etc., and are not limited to these.

[0102] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to illustrate selected embodiments of the invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without inventive effort are within the scope of protection of the present invention.

[0103] To verify the technical advantages of this invention, a simulation experimental platform containing 10 robots was built. Three typical GNSS-deficient scenarios—tunnels, mines, and warehouses—were selected for comparative testing with three existing mainstream solutions (a centralized solution based on three calibrated UWB anchor points, a robot-to-robot mutual observation solution, and a visual marker solution). Key performance indicators are as follows: (1) Deployment and initialization efficiency This invention: Anchor point deployment time ≤ 5 minutes (no calibration required), single robot initialization time ≤ 28 seconds, total time for 10 robots to go online asynchronously ≤ 45 seconds; 3-point UWB anchor point scheme: anchor point calibration + deployment time ≥ 60 minutes, single robot initialization time ≤ 35 seconds, total time for 10 robots to go online simultaneously ≥ 80 seconds; Inter-robot mutual observation scheme: No anchor point deployment time, single robot initialization time ≥ 120 seconds (depending on mutual observation), total time for 10 robots ≥ 240 seconds; Visual marker solution: marker deployment + calibration time ≥ 40 minutes, single robot initialization time ≤ 45 seconds, total time for 10 robots ≥ 90 seconds.

[0104] (2) Initialization precision This invention achieves the following: global coordinate system consistency error ≤ 0.4°, robot global initial pose error ≤ 0.28m, and ranging residual ≤ 0.05m. Three-point calibration UWB anchor point scheme: global coordinate system consistency error ≤ 0.3°, robot global initial pose error ≤ 0.25m, ranging residual ≤ 0.04m; Inter-robot mutual observation scheme: global coordinate system consistency error ≥ 3.2°, robot global initial pose error ≥ 0.85m, no explicit ranging residual index; Visual marker scheme: global coordinate system consistency error ≤ 0.6°, robot global initial pose error ≤ 0.42m, residual fluctuation due to illumination ± 0.1m.

[0105] (3) Environmental adaptability and operating costs In occluded scenarios (multiple pillars in a tunnel): the initialization success rate of this invention is 100%, the success rate of the 3-calibration UWB anchor point solution is 90%, the success rate of the inter-robot mutual observation solution is 65%, and the success rate of the visual marker solution is 70%. In weak network scenarios (bandwidth ≤ 1Mbps): the initialization success rate of this invention is 100% (no cross-machine communication), the success rate of the UWB anchor point calibration scheme is 85% (depending on data aggregation), the success rate of the robot-to-robot mutual observation scheme is 50% (depending on high bandwidth interaction), and the success rate of the visual marker scheme is 90% (low bandwidth requirement). Operating energy consumption: After initialization, the robot disconnects from UWB and its single-unit energy consumption is ≤12W / h; the three-calibrated UWB anchor point scheme continuously relies on UWB and its energy consumption is ≤18W / h; the visual marker scheme continuously relies on the camera and its energy consumption is ≤15W / h.

[0106] Experimental data show that the present invention significantly reduces deployment complexity and initialization time while maintaining high initialization accuracy. Its environmental adaptability and operational economy are significantly better than existing solutions, and it is especially suitable for large-scale collaborative operations of multiple robots in complex GNSS-deficient scenarios.

[0107] This invention has been verified through simulation experiments. The simulation results show that the technical solution of this invention is feasible, and can achieve fast, unique, and robust initialization of the three-dimensional global coordinate system in the target scene, and supports independent parallel and asynchronous deployment of multiple robots; the quality assessment and rollback mechanism effectively ensures the reliability of the results and the reproducibility of engineering.

[0108] In summary, this invention provides a 3D multi-robot localization initialization method and system based on dual UWB anchor points. It requires only two static UWB anchor points, eliminating the need for absolute coordinate calibration, network-wide time synchronization, and environmental markers, significantly reducing initial surveying and subsequent maintenance costs. The entire process is completed within a single robot; short-term non-degenerate motion is sufficient to uniquely construct the global coordinate system and solve the initial pose, resulting in a simple process and rapid deployment. It does not rely on cross-robot data exchange or high-bandwidth links, adapting to complex environments such as occlusion, weak networks, and underground conditions. Each robot initializes independently according to the same rules, with consistent coordinate system definitions. Local maps can be directly stitched together, and collaborative planning requires no additional scale or orientation correction. It supports independent parallel and asynchronous deployment of multiple robots, with unified rules providing natural alignment. After successful initialization, continuous operation no longer relies on UWB ranging, thereby reducing communication and energy consumption, minimizing single-point failures, and facilitating scalability and maintenance management.

[0109] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is merely an example. In practical applications, the above functions can be assigned to different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiments can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit. Furthermore, the specific names of the functional units and modules are only for easy differentiation and are not intended to limit the scope of protection of this application. The specific working process of the units and modules in the above system can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0110] In the above embodiments, the descriptions of each embodiment have different focuses. For parts that are not described in detail or recorded in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0111] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed in this invention can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementations should not be considered beyond the scope of this invention.

[0112] In the embodiments provided by this invention, it should be understood that the disclosed devices / terminals and methods can be implemented in other ways. For example, the device / terminal embodiments described above are merely illustrative. For instance, the division of modules or units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between devices or units may be electrical, mechanical, or other forms.

[0113] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0114] Furthermore, the functional units in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0115] If the integrated module / unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include: any entity or device capable of carrying the computer program code, recording media, USB flash drives, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random-access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc. It should be noted that the content included in the computer-readable medium can be appropriately added or removed according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, computer-readable media do not include electrical carrier signals and telecommunication signals.

[0116] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus, and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0117] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0118] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0119] The above content is only for illustrating the technical concept of the present invention and should not be construed as limiting the scope of protection of the present invention. Any modifications made to the technical solution based on the technical concept proposed in this invention shall fall within the scope of protection of the claims of this invention.

Claims

1. A method for three-dimensional multi-robot positioning initialization based on double UWB anchor points, characterized in that, The method comprises the following steps: S1, deploying two static UWB anchors A1 and A2 in a working environment, rigidly installing a UWB tag, an IMU, and a lidar or a camera on a robot body; S2, collecting static IMU data to estimate a gravity direction g, establishing a ranging channel with the two static UWB anchors A1 and A2, and starting a odometer to output a relative pose; S3, the robot performs a motion containing a change of orientation, synchronously acquiring N sets of observation data, including: the distance of the robot to anchor point A1 , the distance to anchor point A2 , and the pose of the robot in the local coordinate system L output by the odometer , ; S4. Based on the observation data, the position coordinates of the two static UWB anchor points A1 and A2 are solved in the local coordinate system L by an optimization algorithm and ; S5, with position coordinates as the origin, with position coordinates pointing to the position coordinates vector and the direction of gravity g as constraints, according to the right-hand coordinate system rule, a unique three-dimensional global coordinate system G is constructed; S6, converting the initial pose of the robot in the local coordinate system L to the three-dimensional global coordinate system G constructed in step S5 through coordinate transformation, to obtain the initial pose of the robot in the three-dimensional global coordinate system G. 2.The method of claim 1, wherein, In step S1, the two static UWB anchors A1 and A2 do not need to be accurately calibrated in a three-dimensional space coordinate, and their unique IDs are recorded when they are deployed. The two static UWB anchors A1 and A2 are selected to be fixed positions that are not easy to move and have stable line-of-sight, and are at the same horizontal height. After the UWB tag, the IMU, and the lidar or the camera are installed, the external parameter fixing and the timestamp unification are completed to ensure that there is no relative displacement or angle drift during operation. 3.The method of claim 1, wherein, In step S2, 2-3 seconds of static IMU data are collected to estimate a local gravity unit vector g through time averaging processing. The UWB tag establishes a two-way ranging channel with anchors A1 and A2 to obtain real-time distance data. The odometer is a tightly coupled front end of laser and IMU or a tightly coupled front end of vision and IMU. The UWB tag, the IMU, and the odometer are all managed by a single machine unified time source for timestamp management. 4.The method of claim 1, wherein, In step S3, the motion performed by the robot is a short non-degenerate autonomous motion with a small amount of rotation; the observation data further includes the estimated gravity unit vector g in step S2, and all the observation data is time-stamped to form a complete observation sequence ; the local coordinate system L has the position of the robot at i = 1 as the origin. 5.The method of claim 1, wherein, In step S4, the position coordinates of the anchor points A1 and A2 are solved The specific process is as follows: For the collected The sequence is subjected to sliding window filtering; extraction The translation part in the middle, as the first Time Robot in Local Coordinate System The lower position ; Constructing nonlinear observation constraints: ; establishing a least squares optimization question; The least square optimization problem is solved iteratively by Levenberg-Marquardt algorithm to obtain the position coordinates of anchor points A1 and A2 in the local coordinate system . . 6.The method of claim 5, wherein, Nonlinear least squares problem: wherein, is the number of time steps within the sampling window. 7.The method of claim 1, wherein, In step S5, the specific process of constructing the three-dimensional global coordinate system G is as follows: Z-axis direction defining a three-dimensional global coordinate system G ; Computing an anchor baseline vector , normalizing the baseline vector to obtain an initial X-axis unit vector ; The initial X-axis unit vector is subjected to Schmidt orthogonalization processing to remove the Z-axis direction component, and the processed vector is normalized to obtain an X-axis unit vector ; Calculate Y axis unit vector in right-handed coordinate system ; Constructing a rotation matrix , in conjunction with the origin , forming a unique three-dimensional global coordinate system G. 8.The method of claim 1, wherein, In step S6, the transformation matrix of the coordinate transformation is ; the initial pose of the robot in the local coordinate system L is the unit homogeneous transformation , and the initial pose of the robot in the three-dimensional global coordinate system G is . 9.The method of claim 1, wherein, After obtaining the initial pose of the robot in the three-dimensional global coordinate system G, the quality self-evaluation is initialized, specifically as follows: Computing uwb ranging residuals , ; determining , , whether the absolute values of the ranging residuals are all lower than a preset angle tolerance; if all the ranging residuals are smaller than a preset ranging threshold and the orthogonality of the coordinate axes meets the requirements, it is determined that the initialization is successful, the definition of a three-dimensional global coordinate system G and the initial pose of the robot are issued, and the subsequent positioning and mapping no longer continuously rely on UWB ranging; if not, it is rolled back to step S3 to re-execute the motion and data acquisition. 10.The method of claim 1, wherein, Multiple robots independently and in parallel execute steps S1 to S6. Each robot constructs a three-dimensional global coordinate system G according to the unified rules of step S5, realizes direct splicing and collaborative planning of local maps of multiple robots, and does not need cross-robot communication or centralized optimization. 11.A three-dimensional multi-robot positioning initialization system based on double UWB anchor points, characterized in that, It comprises: an anchor module for deploying two static UWB anchors A1 and A2 in a working environment, rigidly installing a UWB tag, an IMU, and a lidar or a camera on a robot body, and completing external parameter fixing and timestamp unification; an initialization module for collecting static IMU data to estimate a gravity direction g, establishing a ranging channel with the two static UWB anchors A1 and A2, and starting an odometer to output a relative pose; The acquisition module is configured to control the robot to perform a motion containing a direction change, and synchronously acquire N sets of observation data, the observation data including: a distance from the robot to an anchor point A1 , a distance from the robot to an anchor point A2 , and a pose of the robot in a local coordinate system L output by the odometer , . a solving module, configured to solve the position coordinates of the two static UWB anchor points A1 and A2 in the local coordinate system L based on the observation data by an optimization algorithm and ; a building module for building a unique three-dimensional global coordinate system G with the position coordinates as origin, with the position coordinates pointing to the position coordinates vector and the gravity direction g as constraints, according to the right-hand coordinate system rule a calculation module for converting the initial pose of the robot in a local coordinate system L to a three-dimensional global coordinate system G through coordinate transformation, to obtain the initial pose of the robot in the three-dimensional global coordinate system G.

12. The dual-UWB anchor based three-dimensional multi-robot positioning initialization system of claim 10, wherein, The global coordinate system construction module comprises an orthogonalization processing unit configured to perform Schmidt orthogonalization processing on the initial X-axis unit vector to remove the Z-axis direction component and ensure the orthogonality of the coordinate axes of the three-dimensional global coordinate system G.

13. The dual-UWB anchor based three-dimensional multi-robot positioning initialization system of claim 11, wherein, The observation data collection module is also used to control the robot to perform short-time non-degenerate autonomous motion containing a small amount of rotation, and to timestamp align the collected observation data to form a complete observation sequence containing a gravity unit vector g.

14. The dual-UWB anchor based three-dimensional multi-robot positioning initialization system of claim 11, wherein, The anchor point position solving module comprises a data preprocessing unit and an optimization solving unit; the data preprocessing unit is used for performing sliding window filtering on UWB ranging data and eliminating abnormal points; the optimization solving unit is used for constructing a nonlinear least square model and solving anchor point position coordinates iteratively by using a Levenberg-Marquardt algorithm and .

15. The dual-UWB anchor based three-dimensional multi-robot positioning initialization system of claim 11, wherein, The quality evaluation and feedback module is further included for calculating UWB ranging residual and coordinate axis orthogonality, determining whether initialization is successful according to a preset ranging threshold and angle tolerance, triggering the global coordinate system and initial pose to be published and switching to a normal positioning mode if successful, and triggering the observation data collection module to re-execute data collection if unsuccessful.

16. A computer-readable storage medium storing one or more programs, the one or more programs comprising instructions for: The one or more programs include instructions that, when executed by a computing device, cause the computing device to perform the method of any of claims 1-10.

17. A computing device, comprising: Comprise: One or more processors, memory, and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, the one or more programs comprising steps for performing the method of any of claims 1-10.

Images