A double-vehicle heterogeneous collaborative mapping method and system based on UWB weak global constraint and reliable perception robust fusion

Abstract

The application discloses a double-vehicle heterogeneous collaborative mapping method and system based on UWB weak global constraint and reliable perception robust fusion, and belongs to the technical field of sensor fusion and factor graph optimization. The method comprises the following steps: constructing a basic observation set and a unified back-end factor graph of double-vehicle heterogeneous collaborative mapping, and performing first-stage basic optimization to obtain double-vehicle initial trajectory estimation results; based on the double-vehicle initial trajectory estimation results, constructing a diagnostic reliability sequence of a double-vehicle odometer mode and a UWB mode, and obtaining a bounded reliability state variable through bounded mapping; constructing a joint optimization cost function comprising an observation residual term, a diagnostic reliability prior term and a bounded reliability state time smoothing term, performing second-stage joint optimization, and providing a pose reference for double-vehicle unified coordinate transformation, splicing or map incremental updating according to the optimized double-vehicle unified trajectory results. The method can improve double-vehicle trajectory estimation stability and map splicing consistency.

Description

Technical Field

[0001] This invention belongs to the field of sensor fusion and factor graph optimization technology, specifically involving a dual-vehicle heterogeneous collaborative mapping method and system based on UWB weak global constraints and robust fusion of reliability perception. Background Technology

[0002] In multi-mobile robot systems used in warehousing and logistics, factory automation, and other scenarios, collaborative mapping is fundamental for formation control and autonomous navigation. Existing distributed methods assign perception and estimation tasks to each robot, but are sensitive to the accuracy and consistency of each modality. In indoor environments where GPS is unavailable, single-vehicle odometers suffer from cumulative drift, and the lack of unified global observation during dual-vehicle fusion can easily lead to relative trajectory deflection and scale mismatch. Ultra-wideband (UWB) ranging can provide absolute distance constraints, but it is significantly affected by line-of-sight (LOS) / non-line-of-sight (NLOS) switching and multipath interference, resulting in significant fluctuations in ranging quality, making it a weak global constraint. Meanwhile, lidar lacks lateral geometric constraints in long corridors and open areas, leading to unreliable odometer outputs. Existing methods also have limitations in handling sensor blind spots and unstructured boundaries.

[0003] Existing collaborative mapping methods either treat UWB as an independent correction parameter or employ fixed robust kernel functions to suppress outliers, lacking a unified perception and online adjustment capability for dynamic changes in the quality of odometer and UWB observations. The weak global constraints of UWB are not fully embedded in the backend optimization core, making it difficult to adaptively handle odometer errors and UWB anomalies in locally degraded regions within the same framework. Therefore, a dual-vehicle collaborative mapping method is needed that embeds weak global constraints of UWB and a reliability perception mechanism into a unified backend optimization. Summary of the Invention

[0004] The purpose of this invention is to provide a dual-vehicle heterogeneous collaborative mapping method and system based on UWB weak global constraints and robust fusion of reliability perception. By constructing diagnostic reliability sequences for vehicle odometer modes and UWB modes, and upgrading reliability from fixed weights outside the graph to explicit state variables inside the graph, diagnostic priors and time smoothing terms are introduced in the joint optimization, so that the constraint contribution of the degraded mode is automatically attenuated and the constraint contribution of the normal mode is automatically enhanced, thus solving the problem of abnormal observation contaminating the optimizer when LOS / NLOS switching and local degradation of lidar.

[0005] To achieve the above objectives, the present invention employs the following technical solution:

[0006] This invention provides a dual-vehicle heterogeneous collaborative mapping method based on the fusion of UWB weak global constraints and reliability-aware robustness, comprising the following steps:

[0007] Acquire multi-source observation data of the first vehicle and the second vehicle, and construct a basic observation set for dual-vehicle heterogeneous collaborative mapping. The multi-source observation data includes at least the odometer data of the first vehicle, the odometer data of the second vehicle, UWB anchor point coordinate information, and UWB ranging data.

[0008] A unified backend factor graph is constructed based on the aforementioned basic observation set, and the first stage of basic optimization is performed to obtain the initial trajectory estimation results of the two vehicles. The unified backend factor graph includes a first vehicle odometer factor, a second vehicle odometer factor, a UWB ranging factor, and a pose prior factor.

[0009] Based on the initial trajectory estimation results of the two vehicles and the multi-source observation data, diagnostic reliability sequences for the first vehicle odometer mode, the second vehicle odometer mode, and the UWB mode are constructed respectively.

[0010] The diagnostic reliability sequence is used as a priori observation, and the original reliability state variables in the graph are introduced. The original reliability state variables in the graph are then constrained within a preset reliability interval by a bounded mapping to obtain the bounded reliability state variables in the graph.

[0011] Construct a joint optimization cost function that includes observation residuals, diagnostic reliability priors, and bounded reliability state-time smoothing terms;

[0012] Based on the joint optimization cost function, the second-stage joint optimization is performed on the dual-vehicle trajectory state and the in-graph bounded reliability state variables of each mode to obtain the optimized dual-vehicle unified trajectory result and the time-varying reliability state sequence of each mode.

[0013] Based on the optimized unified trajectory results of the two vehicles, a pose reference is provided for the unified coordinate transformation, stitching, or incremental map update of the local map or local point cloud data of the two vehicles.

[0014] Preferably, the first vehicle odometer data is generated by a lidar odometer or a lidar-inertial odometer; the second vehicle odometer data is generated by a visual odometer, a visual-inertial odometer, or an RGB-D odometer; the UWB anchor point is an absolute reference node pre-deployed in the environment with known coordinates; and the UWB ranging data is the time-of-flight ranging value between the UWB tag carried by each vehicle and each UWB anchor point.

[0015] Preferably, the basic observation set for constructing the dual-vehicle heterogeneous collaborative mapping includes:

[0016] Based on a unified hardware clock or interpolation strategy, odometer data and UWB ranging data of different frequencies are aligned to the same time axis, and UWB anchor point coordinate information is used as a static calibration parameter. The anchor point is associated with the corresponding UWB ranging data to form a basic observation set for dual-vehicle heterogeneous collaborative mapping.

[0017] Preferably, the construction of a unified backend factor graph based on the basic observation set includes:

[0018] The two-dimensional position and yaw angle estimation represent the two-dimensional pose state of the vehicle at a single moment, and the two-dimensional pose increment transformation information represents the relative motion between pose states at adjacent moments. These are used as inputs to the unified back-end factor graph for odometer observations to construct vehicle odometer factors that connect the pose nodes of the vehicle at adjacent moments. The vehicle odometer factors include a first vehicle odometer factor and a second vehicle odometer factor.

[0019] The two-dimensional pose increment transformation information is represented as follows: , , , , in Represents two-dimensional pose increment transformation information. and This represents the translational component of the displacement in the vehicle's local coordinate system at adjacent time points. This indicates the change in yaw angle between adjacent time points. Used to normalize the angle to a preset angle range. , , They represent the first The car is at the moment The horizontal and vertical components of the two-dimensional pose state, as well as the yaw angle. , , They represent the first The car is at the moment The horizontal and vertical components of the two-dimensional pose state, as well as the yaw angle. ;

[0020] The UWB ranging factor connects the vehicle's UWB tag node and the UWB anchor node, and is used to constrain the Euclidean distance between the vehicle's pose state and the known UWB anchor coordinates.

[0021] Pose prior factor, used to fix the pose of the first frame to eliminate the degrees of freedom of the reference frame.

[0022] Preferably, the first stage of basic optimization to obtain the initial trajectory estimation results for the two vehicles includes:

[0023] The first stage of basic optimization is performed using the Gauss-Newton method or the Levenberg-Marquardt method. The optimization objective is to minimize the weighted sum of squares of the vehicle odometer factor, UWB ranging factor, and pose prior factor to obtain the initial trajectory estimation results for the two vehicles.

[0024] Preferably, the step of constructing diagnostic reliability sequences for the first vehicle odometer mode, the second vehicle odometer mode, and the UWB mode based on the initial trajectory estimation results of the two vehicles and multi-source observation data includes:

[0025] The variance of the translational and rotational increments of the vehicle odometer within the sliding window is statistically analyzed. The ratio of the current window variance to the historical baseline variance is used as the diagnostic value of the vehicle odometer mode. The diagnostic value of the vehicle odometer mode is mapped to the diagnostic reliability sequence of the vehicle odometer mode through a dead zone function or a nonlinear decay function.

[0026] Based on the initial trajectory estimation results of the two vehicles obtained from the first-stage basic optimization, the time step is calculated. The whitened absolute residuals of each UWB ranging edge; the UWB ranging edge refers to the constraint edge formed by a UWB ranging observation in the factor graph;

[0027] Time The mean and maximum terms of the whitening absolute residuals of each UWB ranging edge are weighted and aggregated to obtain the time. UWB modal diagnostic residuals;

[0028] The diagnostic value of the robustly normalized UWB mode is calculated based on the median and median absolute deviation of the diagnostic residuals of the UWB modes at all times.

[0029] The diagnostic quantities of the robustly normalized UWB modes are processed for dead zones and mapped to the original diagnostic reliability of the UWB modes through a preset mapping function.

[0030] The original diagnostic reliability of the UWB mode is smoothed and limited to obtain the diagnostic reliability of the UWB mode. The diagnostic reliability of the UWB mode at all times constitutes the diagnostic reliability sequence of the UWB mode.

[0031] Preferably, the step of using the diagnostic reliability sequence as a priori observation, introducing the original reliability state variables within the graph, and constraining the original reliability state variables within the graph to a preset reliability interval through bounded mapping to obtain bounded reliability state variables within the graph includes:

[0032] Based on the diagnostic reliability sequence, the original reliability state variables within the initial graph are initialized as follows:

[0033] ,

[0034] ,

[0035] in, This represents the original reliability state variables within the diagram. Indicates a diagnostic reliability sequence. Indicates will Normalization to Intermediate variables after the interval, Indicates the lower limit of reliability. A positive number used for amplitude limiting. This indicates a limit operation. Represents the natural logarithm function;

[0036] The original reliability state variables in the graph include: the original reliability state variables in the graph for the first vehicle odometer mode, the second vehicle odometer mode, the first vehicle UWB mode, and the second vehicle UWB mode.

[0037] The original reliability state variables in the graph are mapped to a range from a preset reliability lower limit to 1 using the Sigmoid function, resulting in bounded reliability state variables in the graph. The bounded reliability state variables in the graph approach the preset reliability lower limit when the original reliability state variables in the graph approach negative infinity, and approach 1 when the original reliability state variables in the graph approach positive infinity.

[0038] Preferably, the construction of the joint optimization cost function, comprising the observation residual term, the diagnostic reliability prior term, and the bounded reliability state-time smoothing term, includes:

[0039] ,

[0040] in, To jointly optimize the cost function, For the set of trajectory states of two vehicles, This represents the original set of reliability state variables within the diagram. For the first The a priori residuals of the vehicle's position and orientation. For the first Information matrix of prior factors for parking position pose. For the first Information matrix of vehicle odometer residuals For the first The car is at the moment Vehicle odometer residuals, For the first The car is at the moment For the Distance residuals of each UWB anchor point The information weights for UWB ranging residuals, For the first The vehicle's UWB ranging observation set, and The first The car is at the moment and time The in-circuit bounded reliability state variables of the odometer mode. and The first The car is at the moment and time The graph-bounded reliability state variables of the UWB mode. and The first The car is at the moment The reliability of odometer diagnostics and the reliability of UWB diagnostics. and These represent the prior weights for diagnostic reliability in the vehicle odometer mode and the UWB mode, respectively. and These are the weights of the bounded reliability state-time smoothing term for the vehicle odometer mode and the UWB mode, respectively. These are the balanced weighting coefficients for the prior terms of UWB modal reliability diagnosis.

[0041] Preferably, the second-stage joint optimization of the dual-vehicle trajectory state and the in-graph bounded reliability state of each mode based on the joint optimization cost function, to obtain the optimized unified trajectory result of the dual vehicles and the time-varying reliability state sequence of each mode, includes:

[0042] The Levenberg-Marquardt method is used to iteratively solve the joint optimization cost function, and the dual-vehicle trajectory state set is updated simultaneously in each iteration. With the original set of reliability variables in the figure The corresponding in-graph bounded reliability state variables are obtained through bounded mapping until the decrease in the joint optimization cost function is less than a preset threshold or the maximum number of iterations is reached. The optimized first vehicle trajectory, second vehicle trajectory and corresponding time-varying reliability state sequence are then output.

[0043] This invention also provides a dual-vehicle heterogeneous collaborative mapping system based on UWB weak global constraints and reliability-aware robust fusion, used to implement the above-mentioned dual-vehicle heterogeneous collaborative mapping method based on UWB weak global constraints and reliability-aware robust fusion. The system includes:

[0044] The data acquisition module is used to acquire multi-source observation data of the first vehicle and the second vehicle, and to construct a basic observation set for dual-vehicle heterogeneous collaborative mapping. The multi-source observation data includes at least the odometer data of the first vehicle, the odometer data of the second vehicle, UWB anchor point coordinate information, and UWB ranging data.

[0045] The factor graph construction module constructs a unified backend factor graph based on the basic observation set, including a first vehicle odometer factor, a second vehicle odometer factor, a UWB ranging factor, and a pose prior factor.

[0046] The first-stage optimization module is used to perform the first-stage basic optimization on the unified backend factor graph to obtain the initial trajectory estimation results of the two vehicles.

[0047] The reliability diagnosis module is used to construct diagnostic reliability sequences for the first vehicle odometer mode, the second vehicle odometer mode, and the UWB mode based on the initial trajectory estimation results of the two vehicles and the multi-source observation data.

[0048] The reliability state modeling module is used to take the diagnostic reliability sequence as a priori observation, introduce the original reliability state variables in the graph, and constrain the original reliability state variables in the graph within a preset reliability interval through bounded mapping to obtain the bounded reliability state variables in the graph.

[0049] The joint optimization module is used to construct a joint optimization cost function that includes observation residuals, diagnostic reliability priors, and bounded reliability state-time smoothing terms.

[0050] The output module is used to perform a second-stage joint optimization on the trajectory states of the two vehicles and the intra-graph bounded reliability state variables of each mode based on the joint optimization cost function, so as to obtain the optimized unified trajectory results of the two vehicles and the time-varying reliability state sequences of each mode; and, based on the optimized unified trajectory results of the two vehicles, to provide a pose reference for the unified coordinate transformation, stitching or incremental map update of the local map or local point cloud data of the two vehicles.

[0051] The beneficial effects achieved by this invention are as follows:

[0052] This invention introduces UWB ranging as a weak global constraint into a unified back-end factor graph for both vehicles, enabling continuous correction of odometer cumulative drift with an absolute scale benchmark, thus solving the problems of trajectory divergence and map stitching misalignment during long-range operations. By constructing diagnostic reliability sequences for vehicle odometer and UWB modes, and upgrading reliability from fixed weights outside the graph to explicit state variables within the graph, diagnostic priors and time smoothing terms are introduced into the joint optimization. This automatically attenuates the constraint contribution of degraded modes and automatically enhances the constraint contribution of normal modes, addressing the problem of abnormal observations contaminating the optimizer during LOS / NLOS switching and localized LiDAR degradation.

[0053] To address the issue that when a single UWB reliability state corresponds to multiple UWB ranging edges at the same time, multiple ranging residuals can collectively affect the same UWB reliability state, leading to an abnormal pull on the UWB reliability state towards a preset lower bound, this invention introduces a balanced weighting mechanism based on the number of ranging edges. This mechanism weights the UWB reliability diagnostic prior residuals according to the square root of the number of ranging edges, ensuring that the information strength of the diagnostic prior in the squared cost matches the number of UWB ranging edges at the same time. This approach can suppress abnormal collapse of the UWB reliability state under conditions of UWB link quality fluctuations, LiDAR geometric degradation, and partial anchor point visibility, thereby improving the numerical stability of dual-vehicle trajectory estimation and map stitching consistency. Attached Figure Description

[0054] Figure 1 This is a schematic diagram of the process of a dual-vehicle heterogeneous collaborative mapping method based on UWB weak global constraints and robust fusion of reliability perception provided in Embodiment 1 of the present invention.

[0055] Figure 2 This is a schematic diagram of the unified backend factor graph model structure constructed in Embodiment 1 of the present invention;

[0056] Figure 3 This is a schematic diagram of the diagnostic reliability mapping function curve in Embodiment 1 of the present invention;

[0057] Figure 4 This is a schematic diagram comparing the dual-vehicle trajectory with the actual trajectory in a long corridor degradation scenario using the existing technology that relies solely on the odometer constraint method;

[0058] Figure 5 This is a schematic diagram comparing the dual-vehicle trajectory with the actual trajectory in a long corridor degradation scenario using the method of Embodiment 1 of the present invention;

[0059] Figure 6 It corresponds to Figure 4 A schematic diagram showing the variation of the relative error of the two vehicle trajectories over time during the simulation.

[0060] Figure 7 It corresponds to Figure 5 A schematic diagram showing the variation of the relative error of the two vehicle trajectories over time during the simulation.

[0061] Figure 8 This is a schematic diagram comparing the dual-vehicle trajectory with the actual trajectory in a scenario where some UWB anchor points are visible and NLOS interference exists, using the method of Embodiment 1 of the present invention;

[0062] Figure 9 It corresponds to Figure 8 A schematic diagram showing the change in UWB reliability status over time during the simulation. Detailed Implementation

[0063] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. The terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the scope of this application.

[0064] 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 embodiments of the present invention, and not all embodiments. Based on the embodiments in this application, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of this application.

[0065] Example 1

[0066] To overcome the problems of long-term drift suppression, insufficient utilization of UWB weak global constraints, and difficulty in uniformly handling multimodal degradation in existing technologies, this embodiment 1 provides a dual-vehicle heterogeneous collaborative mapping method based on the fusion of UWB weak global constraints and reliability-aware robustness. Figure 1 As shown, it includes the following steps:

[0067] Step S1: Acquire multi-source observation data from the first and second vehicles, and construct the basic observation set for dual-vehicle heterogeneous collaborative mapping. The multi-source observation data includes at least the odometer data from the first and second vehicles, UWB anchor point coordinates, and corresponding UWB ranging data. This step acquires the heterogeneous odometer data and UWB ranging data from both vehicles, performs time alignment and unified organization, providing synchronized observation input for subsequent unified backend factor map construction, including:

[0068] Step S11: Obtain the first odometer data of the first vehicle and the second odometer data of the second vehicle, including:

[0069] The first vehicle odometer data is generated by a lidar odometer or a lidar-inertial odometer, and the second vehicle odometer data is generated by a visual odometer, a visual-inertial odometer, or an RGB-D odometer; the UWB anchor points are pre-deployed absolute reference nodes in the environment with known coordinates; the UWB ranging data are the time-of-flight ranging values ​​between the UWB tags on each vehicle and each UWB anchor point.

[0070] In one embodiment, the first vehicle is equipped with a lidar and an inertial measurement unit (IMU), generating two-dimensional position and yaw angle estimates using a lidar odometry method; the second vehicle is equipped with a vision sensor and an IMU, generating two-dimensional position and yaw angle estimates using a visual odometry method. Both the first and second odometry data contain incremental transformation information of the two-dimensional pose. The output frequency can be determined according to the actual sensor configuration, for example, 10Hz to 20Hz.

[0071] The two-dimensional position and yaw angle estimates represent the vehicle's two-dimensional pose state in the corresponding odometer coordinate system or world coordinate system, denoted as . , Indicates the first The car is at the moment The pose state vector, which consists of the horizontal coordinate component , ordinate components and yaw angle Composition. Two-dimensional pose increment transformation information. The relative motion between two-dimensional pose states at adjacent time points is denoted as . , and This represents the translational component of the displacement in the vehicle's local coordinate system at adjacent time points. It represents the change in yaw angle between adjacent moments. It can be directly output by the odometer calculation method, or it can be obtained by estimating the difference between the two-dimensional position and yaw angle between adjacent moments and converting it to the vehicle's local coordinate system.

[0072] Therefore, the two-dimensional position and yaw angle estimates are the pose results at each time step, and the incremental transformation information of the two-dimensional pose is a relative constraint observation derived from the pose results at adjacent time steps, which is used for the construction of odometry factors in the subsequent unified back-end factor graph.

[0073] Step S12: Obtain the coordinate information of pre-deployed UWB anchor points in the environment and the corresponding UWB ranging data, including:

[0074] Four UWB anchor points are deployed in the indoor warehouse area. The anchor point positions are obtained by pre-calibrating the world coordinates. The first vehicle and the second vehicle are each equipped with a UWB tag. The tag outputs the time-of-flight ranging value between itself and each UWB anchor point at a certain frequency (e.g., 20Hz). The UWB ranging data consists of the time-of-flight ranging value between the UWB tag on each vehicle and each UWB anchor point. A UWB observation set corresponding to the pose at each time moment is constructed. Each observation record includes a timestamp, tag ID, anchor point ID and ranging value.

[0075] In one embodiment, the world coordinate system of the indoor warehouse area is set with one corner of the site as the origin, and the coordinates of the four UWB anchor points are respectively set as follows: ,in and These represent the length and width of the indoor warehouse area, respectively. These coordinates can also be obtained using a total station, laser rangefinder, or manual calibration.

[0076] Step S13: Perform time correlation and unified organization on the first odometer data, the second odometer data, the UWB anchor point coordinate information, and the UWB ranging data, including:

[0077] Based on a unified hardware clock or software interpolation strategy, odometer data and UWB ranging data of different frequencies are aligned to the same time axis, and UWB anchor point coordinate information is used as a static calibration parameter. The anchor point is associated with the corresponding UWB ranging data to form a basic observation set for dual-vehicle heterogeneous collaborative mapping.

[0078] In one embodiment, based on a unified hardware clock, a timestamp nearest neighbor matching strategy is used to align 10Hz laser odometry data, 15Hz visual odometry data, and 20Hz UWB ranging data to the same time axis at the software level. At the same time, UWB anchor point coordinate information is used as a static calibration parameter, and a correlation is established between the anchor point identifier and the aligned UWB ranging data, so that each UWB ranging observation corresponds to a UWB anchor point with known coordinates, forming the basic observation set for dual-vehicle heterogeneous collaborative mapping.

[0079] Step S2: Construct a unified backend factor graph based on the basic observation set established in Step S1, and perform the first stage of basic optimization to obtain the initial trajectory estimation results for both vehicles. The unified backend factor graph includes the first vehicle odometer factor, the second vehicle odometer factor, the UWB ranging factor, and the pose prior factor. This step, by constructing a factor graph containing the vehicle odometer factor, UWB ranging factor, and pose prior factor, and performing initial optimization, provides a reference trajectory for subsequent diagnostic reliability calculations, including:

[0080] Step S21: Using the dual-vehicle trajectories as the state to be estimated, construct a unified back-end factor graph containing the first vehicle's odometer factor, the second vehicle's odometer factor, the UWB ranging factor, and the pose prior factor, including:

[0081] The set of state nodes in the factor graph contains the pose states of the first vehicle and the second vehicle at each time step. Each pose state includes the vehicle's position and yaw angle in the world coordinate system, represented as:

[0082] ,

[0083] ,

[0084] In the formula, Indicates the first The car is at the moment The two-dimensional pose state vector consists of the horizontal coordinate component. , ordinate components and yaw angle Composition; among which, The vector symbol is in bold. and This is a non-bold scalar symbol; Indicates the first The car is at the moment The two-dimensional position vector, by and composition; Indicates the first The car is at the moment Yaw angle; .

[0085] When the odometer front end outputs two-dimensional position and yaw angle estimates at each moment, the first... The car is at the moment At that time The two-dimensional pose increment transformation information is calculated from the two-dimensional pose states at adjacent time points. Specifically, let the first... The car is at the moment The two-dimensional pose state vector is At any moment The two-dimensional pose state vector is Then the two-dimensional pose increment transformation information It can be obtained from the following formula:

[0086] ,

[0087] ,

[0088] ,

[0089] in, and This represents the translational component of the displacement in the vehicle's local coordinate system at adjacent time points. This indicates the change in yaw angle between adjacent time points. Used to normalize the angle to a preset angle range.

[0090] Therefore, the two-dimensional position and yaw angle estimation represents the vehicle's pose state at a single moment, and the two-dimensional pose increment transformation information represents the relative motion between pose states at adjacent moments. It serves as the input of the odometer observation into the unified back-end factor graph, which is used to construct the vehicle odometer factor connecting the pose nodes at adjacent moments.

[0091] In another embodiment, when the lidar odometry, lidar-inertial odometry, visual odometry, visual-inertial odometry, or RGB-D odometry directly outputs the relative pose increment between adjacent moments, the relative pose increment is directly used as the two-dimensional pose increment transformation information.

[0092] Let the first The car is at the moment The odometer measurement is The predicted relative pose is The vehicle odometer residual is Represented as:

[0093] ,

[0094] ,

[0095] in, This represents the angle normalization operation, used to normalize the input angle according to... Periodic mapping is applied to a preset principal value range.

[0096] In one implementation, It can be represented as:

[0097] ,

[0098] Make the output angle located at Within the range; angle error passes Normalize to a preset angle range. , , The first The car is at the moment The x-axis component, y-axis component, and yaw angle.

[0099] The first vehicle odometer factor is connected to the pose nodes of the first vehicle at adjacent time points and is used to constrain the relative pose transformation of the first vehicle at adjacent time points; the second vehicle odometer factor is connected to the pose nodes of the second vehicle at adjacent time points and is used to constrain the relative pose transformation of the second vehicle at adjacent time points.

[0100] Let the first The car is at the moment For the first The distance measurement observation value of each UWB anchor point is The predicted distance and the UWB ranging residual are then:

[0101] ,

[0102] ,

[0103] In the formula, For the first The coordinates of a UWB anchor point in the world coordinate system. For the first The car is at the moment For the Distance observations of each UWB anchor point, The predicted distance is calculated based on the vehicle's location and anchor point coordinates. For UWB ranging residuals; The L2 norm of a vector is used to calculate the two-dimensional position of a vehicle. With the UWB anchor point coordinates The Euclidean distance between them.

[0104] The UWB ranging factor connects the vehicle's UWB tag node and the UWB anchor node, and is used to constrain the Euclidean distance between the vehicle's pose state and the known UWB anchor coordinates.

[0105] Pose prior factor, used to fix the pose of the first frame to eliminate the degree of freedom of the reference frame.

[0106] Step S22: Perform the first stage of basic optimization based on the unified backend factor graph described above, including:

[0107] The Gauss-Newton method or the Levonberg-Marquardt method is used to perform a first-stage basic optimization on the unified back-end factor graph without introducing explicit reliability state variables in the graph, and the initial trajectory estimation results of the two vehicles are obtained.

[0108] In one embodiment, the Levenberg-Marquardt method is used to perform the first stage of basic optimization. This first stage of basic optimization does not introduce explicit in-graph reliability state variables; its optimization objective is to minimize the weighted sum of squares of the vehicle odometer factor, UWB ranging factor, and pose prior factor to obtain the initial trajectory estimation results for both vehicles. .

[0109] Among them, the pose prior factor is used to constrain the initial pose of the vehicle to eliminate the degrees of freedom of the factor graph reference frame. Let the first... The initial pose prior of the car is , No. The vehicle's pose to be estimated at the initial moment is Then the first a priori residuals of vehicle position Represented as:

[0110] ,

[0111] In the formula, , , The first The x-coordinate components, y-coordinate components, and yaw angle in the initial pose prior of the vehicle This indicates the angle normalization operation.

[0112] Initial trajectory estimation results of dual vehicles Represented as:

[0113] ,

[0114] In the formula, The set of dual-vehicle trajectory state variables to be optimized includes the pose states of the first and second vehicles at each time step, which are unknown optimization variables in the first stage of basic optimization. The initial trajectory estimation results of the two vehicles obtained when the above objective function reaches its minimum value are used for subsequent diagnostic reliability sequence generation, and are not the true trajectories known in advance; This operator represents the set of dual-vehicle trajectory state variables that minimizes the weighted sum of squares objective function within the parentheses. For the first Information matrix of vehicle odometer residuals The information weights for UWB ranging residuals, The first one calculated according to the above formula Prior residuals of the vehicle's initial pose For the first Information matrix of prior factors for parking position pose. For the first The vehicle's UWB ranging observation set.

[0115] Figure 2 The unified backend factor graph model structure constructed for embodiments of the present invention. Figure 2 In the second stage of joint optimization, the two-dimensional poses of the first and second vehicles at each time step serve as state nodes to be optimized, with adjacent poses connected by their respective odometry factors. UWB anchor points with known coordinates in the environment provide weak global constraints to the dual-vehicle pose nodes through UWB ranging factors. The first frame pose prior factor is used to constrain the global coordinate reference. The initial trajectory obtained from the first-stage basic optimization is further used to generate diagnostic reliability priors for the odometry mode and UWB mode. In the second stage joint optimization, the diagnostic reliability prior, in-graph reliability state variables, and reliability state time smoothing terms work together to dynamically adjust the constraint contributions of various observation factors according to observation quality. For cases where multiple UWB ranging edges share the same UWB reliability state at the same time step, the square root of the number of ranging edges is used to enhance the weight of the diagnostic reliability prior to suppress abnormal collapse of the reliability state.

[0116] Step S3: Based on the initial trajectory estimation results of the two vehicles and the multi-source observation data, construct diagnostic reliability sequences for the first vehicle odometer mode, the second vehicle odometer mode, and the UWB mode, respectively. This step calculates the diagnostic quantities of the vehicle odometer mode and the UWB mode and maps them to the corresponding diagnostic reliability sequences, including:

[0117] The diagnostic quantities of the first vehicle odometer mode and the second vehicle odometer mode are determined by the local variance ratio or the consistency of the mode itself. Specifically, the variance of the translation increment and rotation increment of the vehicle odometer within the sliding window is statistically analyzed, and the ratio of the current window variance to the historical baseline variance is used as the diagnostic quantity of the vehicle odometer mode. The diagnostic quantity of the vehicle odometer mode is mapped to the diagnostic reliability sequence of the vehicle odometer mode through a dead zone function or a nonlinear decay function.

[0118] The diagnostic quantity of the UWB mode is determined by the ranging residual aggregation result. Specifically, based on the initial trajectory estimation result of the two vehicles obtained from the first stage of basic optimization, the whitening absolute residual of each UWB ranging edge is calculated, the residual sequence within the sliding window is statistically aggregated, and the aggregation result is corrected by combining the number of visible anchor points at the current time, LOS / NLOS scene identifiers or geometric precision factors to obtain the corrected diagnostic quantity of the UWB mode. Then, the corrected diagnostic quantity of the UWB mode is mapped to the diagnostic reliability sequence of the UWB mode through a preset mapping function.

[0119] For the vehicle odometer mode, in one embodiment, the degree of local motion fluctuation is statistically analyzed based on the translational and rotational increments of the vehicle odometer within a sliding window. The current window statistics are compared with historical baseline statistics to obtain the diagnostic parameters of the vehicle odometer mode. Then, the diagnostic parameters of the vehicle odometer mode are mapped to a diagnostic reliability sequence of the vehicle odometer mode using a dead-zone function or a nonlinear decay function, expressed as:

[0120] ,

[0121] In the formula, For the first The car is at the moment The odometer translational incremental model length, For the first The car is at the moment The odometer's two-dimensional translation increment vector includes the values ​​along the vehicle's local coordinate system. direction and Incremental translation in direction For the first The car is at the moment The absolute value of the odometer rotation increment, For the first The car is at the moment The odometer rotation increment, i.e., the yaw angle increment between adjacent moments, For time A sliding window centered on the center. For the time index within the sliding window or reference interval, and The first The car is at the moment The absolute values ​​of the translational increment and rotational increment of the odometer. and The first The car is in the sliding window The standard deviation of translation increment and the standard deviation of rotation increment within the range, This serves as a reference interval for statistically analyzing historical baselines. and The first The car is in the reference range Inner Time The standard deviations of translation and rotation increments correspond to the standard deviations of the sliding window. and The first The car is in the reference range The median of the standard deviation series of internal translation increments and the standard deviation series of rotation increments are used as historical baseline statistics. Represents the standard deviation function. This represents the median function.

[0122] The diagnostic parameters for the vehicle odometer mode are:

[0123] ,

[0124] in, For the first The car is at the moment The odometer translation diagnostic component is used to characterize the excess of the translation increment fluctuation within the current sliding window relative to the historical baseline translation fluctuation. For the first The car is at the moment The odometer rotation diagnostic component is used to characterize the degree of excess of the rotation increment fluctuation within the current sliding window relative to the historical baseline rotation fluctuation. Used to truncate diagnostic components that are less than zero to zero; , , and The meaning is consistent with the aforementioned definition, in which and The first The car is at the moment The standard deviations of the translation increment and rotation increment within the corresponding sliding window, and The first The median of the standard deviation series of translational increments and the standard deviation series of rotational increments within the reference interval is used as the historical baseline statistic. To prevent positive numbers with a denominator of zero; For the first The car is at the moment The diagnostic parameters of the vehicle odometer mode are weighted and synthesized from translational and rotational diagnostic components. These are the weighting coefficients for the rotational diagnostic component, used to adjust the contribution of rotational increment fluctuations to the diagnostic quantities of the vehicle odometer mode. The diagnostic quantity for the effective vehicle odometer mode after dead zone processing. The odometer diagnostic dead zone threshold is used to filter out small, normal fluctuations.

[0125] Let the lower limit of reliability be The odometer diagnostic dead zone threshold is The nonlinear decay exponent is ,but:

[0126] ,

[0127] In the formula, For the first The car is at the moment The reliability of odometer diagnostics As the lower limit of reliability, It is a non-linear decay exponent.

[0128] For the UWB mode, the whitened absolute residuals of each UWB ranging edge are calculated based on the results of the first-stage basic optimization. A UWB ranging edge refers to a constraint edge formed by a UWB ranging observation in the factor graph, which connects the first... The car is at the moment pose node and the first A UWB anchor node or its coordinate parameters are used to constrain the Euclidean distance between the vehicle position and the UWB anchor point.

[0129] In one embodiment, the whitening absolute residual is first calculated for each UWB ranging edge:

[0130] ,

[0131] In the formula, For the first The car is at the moment For the The whitened absolute residuals of the UWB anchor point ranging residuals correspond to a single observation. The constructed UWB ranging edge; According to the first The car is at the moment Position and number The predicted distance is calculated from the coordinates of each UWB anchor point; For the first The car is at the moment For the Distance measurement values ​​of one UWB anchor point; The standard deviation of UWB ranging noise.

[0132] If the first The car is at the moment If a visible UWB ranging observation exists, then the set of anchor point indices corresponding to the visible UWB ranging edge at that moment is denoted as . Each of them Indicates a time For the The range measurement observations or range measurement edges formed by each UWB anchor point; the number of range measurement edges in this set is denoted as . ,but:

[0133] when hour:

[0134] ,

[0135] when Time: The UWB modal diagnostic residual at that moment is obtained by weighted aggregation of the mean and maximum values ​​of the whitening absolute residuals of multiple UWB ranging edges, expressed as:

[0136] ,

[0137] in, For the first The car is at the moment The set of anchor point indices corresponding to the UWB ranging edge is visible; For UWB anchor index; For set The number of elements in, i.e., the number of elements in the first place The car is at the moment The number of visible UWB ranging edges; For the first The car is at the moment For the The absolute value of whitening of the distance measurement residuals at each UWB anchor point; For the first The car is at the moment The UWB modal diagnostic residuals obtained by aggregation; This represents the arithmetic mean operation, i.e., the operation on a set. Average the whitening absolute residuals corresponding to all visible UWB ranging edges.

[0138] Let the median and median absolute deviation of the full-time UWB modal diagnostic residual sequence be respectively:

[0139] ,

[0140] In the formula, The median of the UWB modal diagnostic residual sequence. This represents the absolute deviation of the median. This is the scaling adjustment coefficient. This is the diagnostic metric for the robustly normalized UWB mode.

[0141] Let the UWB diagnostic dead zone threshold be... The decrease coefficient is ,but:

[0142] ,

[0143] In the formula, This is the diagnostic quantity for the robustly normalized UWB mode; This is the diagnostic quantity of the effective UWB modality after dead zone processing, used to indicate the value exceeding the UWB diagnostic dead zone threshold. In the abnormal diagnosis section, when hour, ,when hour, ; Determine the dead zone threshold for UWB diagnosis; This represents the UWB reliability degradation factor. To determine the original diagnostic reliability of the UWB mode obtained by mapping the diagnostic quantity based on the effective UWB mode; For length is The sliding window smoothing operator is used to perform temporal smoothing on the original diagnostic reliability sequence of the UWB mode; To improve the diagnostic reliability of smoothed and limited UWB modes, Constituting the first Diagnostic reliability sequence for vehicle's UWB modes; This represents the lower limit of reliability. This indicates a limiting operation, used to restrict the input value to a certain range. Within the range.

[0144] In one embodiment, the diagnostic reliability of the UWB mode is further multiplied by a visibility correction term, assuming the total number of anchor points is... ,but:

[0145] ,

[0146] ,

[0147] when hour:

[0148] ,

[0149] In the formula, This represents the total number of UWB anchor points. The lower bound coefficient of the visibility correction term. For the first The car is at the moment UWB visibility correction factor For the first The car is at the moment Diagnostic reliability of the modified UWB mode.

[0150] In LOS scenarios, the geometry correction can be turned off.

[0151] In one embodiment, the lower reliability limit The value ranges from 0.05 to 0.3; odometer diagnostic dead zone threshold. The value ranges from 0.1 to 1.0; UWB diagnostic dead zone threshold. The value ranges from 0.1 to 1.5; Odometer reliability degradation index. The value ranges from 1 to 4; UWB reliability degradation coefficient The value ranges from 0.5 to 5; the sliding window length The value ranges from 3 to 30 frames; the lower limit coefficient of the visibility correction term. The value ranges from 0.2 to 0.8.

[0152] Figure 3 This is a schematic diagram of the diagnostic reliability mapping function curve in an embodiment of the present invention. Figure 3 Figure (a) shows the mapping relationship between diagnostic values ​​for the vehicle odometer mode, diagnostic values ​​for the UWB mode, and reliability values. Figure 3 Figure (b) shows how unbounded primitive reliability state variables are constrained to a reliability interval through a bounded mapping. The relationship between [1] and [2]. This mapping is used to convert observation residuals, local consistency, or residual abnormality into reliability states, so that the corresponding factors maintain a strong constraint when the observation quality is high, while the weights of the corresponding factors automatically decay when the observation quality deteriorates or NLOS interference occurs.

[0153] Step S4: Using the diagnostic reliability sequence as a priori observation, introduce the original in-graph reliability state variables corresponding to each modality, thereby upgrading reliability from fixed weights outside the graph to explicit in-graph state variables; the original in-graph reliability state variables are unbounded optimization variables without bounded mapping, and bounded in-graph reliability state variables are obtained through bounded mapping, so that the bounded in-graph reliability state variables are constrained within a preset reliability interval, including:

[0154] An in-graph original reliability state variable is introduced, and then mapped to a range from a preset lower limit to 1 using the Sigmoid function or other bounded transformations, resulting in an in-graph bounded reliability state variable. This in-graph bounded reliability state variable approaches the preset reliability lower limit as the in-graph original reliability state variable approaches negative infinity, and approaches 1 as the in-graph original reliability state variable approaches positive infinity.

[0155] In one embodiment, an in-figure original reliability state variable is introduced. These correspond to the first vehicle odometer mode, the second vehicle odometer mode, the first vehicle UWB mode, and the second vehicle UWB mode, respectively, and are mapped to bounded reliability state variables through the Sigmoid function:

[0156] ,

[0157] ,

[0158] Initialize the original reliability state variables within the graph using the diagnostic reliability sequence generated in step S3:

[0159] ,

[0160] ,

[0161] In the formula, This represents the Sigmoid mapping function, used to map the original reliability state variables within the graph. Mapped to interval; This represents the original reliability state variables within the graph, which are optimization variables that have not undergone bounded mapping, including... ; Indicates by The bounded reliability state variable obtained after the sigmoid bounded mapping has a range of values. ; Indicates the lower limit of reliability; This represents the diagnostic reliability value in the diagnostic reliability sequence generated in step S3, which can be used for odometer diagnostic reliability. or UWB mode diagnostic reliability ; This indicates the diagnostic reliability value. Normalization to The intermediate variables after the interval are used to initialize the original reliability state variables within the graph using the inverse Sigmoid function. ; A positive number used for amplitude limiting to prevent Taking 0 or 1 results in Unable to calculate; This indicates a limit operation. This represents the natural logarithm function.

[0162] Step S5: Construct a joint optimization cost function comprising an observation residual term, a diagnostic reliability prior term, and a bounded reliability state-time smoothing term. The observation residual term includes the first vehicle odometer residual, the second vehicle odometer residual, and the UWB ranging residual. The first vehicle odometer residual is weighted according to the in-graph bounded reliability state variables corresponding to the first vehicle odometer mode. The second vehicle odometer residual is weighted according to the in-graph bounded reliability state variables corresponding to the second vehicle odometer mode. The UWB ranging residual is weighted according to the in-graph bounded reliability state variables of the corresponding vehicle's UWB mode. The diagnostic reliability prior term is used to constrain the deviation between the in-graph bounded reliability state variables and the diagnostic reliability sequence. The bounded reliability state-time smoothing term is used to constrain the change amplitude of the in-graph bounded reliability state variables of the same mode at adjacent time points.

[0163] Construct a joint optimization cost function in the following form:

[0164] ,

[0165] in:

[0166] ,

[0167] In the formula, For the set of trajectory states of two vehicles, This represents the set of original reliability state variables within the diagram. For the first The vehicle's a priori position residuals; For the first The car is at the moment Vehicle odometer residuals; For the first The car is at the moment For the first Distance residuals of each UWB anchor point; For the first The car is at the moment The original reliability state variables within the graph of the odometer mode. For the first The car is at the moment The in-graph primitive reliability state variables of the UWB mode; This is a bounded reliability mapping function used to map the original reliability state variables within a graph to bounded reliability state variables within a preset reliability interval. Represents the original reliability state variables of the odometer mode. The in-graph bounded reliability state variables of the mapped odometer modes, Represents the in-plot primitive reliability state variables of the UWB mode. The in-graph bounded reliability state variables of the mapped UWB modes, and The first The car is at the moment and time The in-plot bounded reliability state variables of the odometer mode; and The first The car is at the moment and time The graph-bounded reliability state variables of the UWB mode; and The generated in step S3 are respectively The car is at the moment The reliability of odometer diagnostics and the reliability of UWB diagnostics; and These are the prior weights for diagnostic reliability in the vehicle odometer mode and the UWB mode, respectively. and These are the weights of the bounded reliability state-time smoothing terms for the vehicle odometer mode and the UWB mode, respectively. For the first The car is at the moment The number of UWB ranging edges, i.e., the number of edges at that moment. The number of UWB ranging observations visible from the vehicle; These are the balancing weighting coefficients for the UWB modal reliability diagnostic priors, used to enhance the constraint strength of the diagnostic priors when multiple UWB ranging edges correspond to the same UWB bounded reliability state variable at the same time.

[0168] Step S6: For cases where a single UWB reliability state corresponds to multiple UWB ranging edges at the same time, the weight of the UWB reliability diagnosis prior term is increased according to the square root of the number of UWB ranging edges at that time, so as to suppress abnormal collapse of the UWB reliability state.

[0169] Let the first The car is at the moment The number of sides for UWB ranging is ,but:

[0170] ,

[0171] ,

[0172] In the formula, These are the base weights for the reliability prior terms in UWB modal diagnosis, used to adjust the in-graph bounded reliability state variables of the UWB modes. UWB diagnostic reliability The constraint strength of the deviation between them in the joint optimization cost function; For the first The car is at the moment The UWB reliability diagnosis prior residual is used as a UWB diagnosis reliability prior term in the joint optimization cost function to participate in the second stage of joint optimization. This residual is used to constrain the in-graph bounded reliability state variables of the UWB mode to deviate from the UWB diagnosis reliability sequence obtained from observation diagnosis.

[0173] By multiplying the prior residual by This makes the information intensity of the prior terms in the squared cost approximately proportional to the number of UWB ranging edges.

[0174] Through the above steps S3 to S6, the construction of the diagnostic reliability of each modality and the establishment of the joint optimization model of the explicit state in the graph are completed. The reliability is upgraded from the external weight to the internal state variable, and a balanced weighting mechanism is introduced to suppress the abnormal collapse of the UWB reliability state.

[0175] Step S7: Based on the joint optimization cost function constructed in Step S5, perform a second-stage joint optimization on the dual-vehicle trajectory state and the in-graph bounded reliability state of each mode, obtaining the optimized unified trajectory result of the dual vehicles and the time-varying reliability state sequence of each mode, including:

[0176] The Levenberg-Marquardt method is used to iteratively solve the joint optimization cost function constructed in step S5, and the dual-vehicle trajectory state set is updated simultaneously in each iteration. With the original set of reliability state variables in the diagram The corresponding in-graph bounded reliability state variables are obtained through bounded mapping. The optimization continues until the decrease in the joint optimization cost function is less than a preset threshold or the maximum number of iterations is reached. Then, the optimized first vehicle trajectory, second vehicle trajectory, and corresponding time-varying reliability state sequences are output.

[0177] In one embodiment, the maximum number of iterations for the second-stage joint optimization is set to 20 to 100; when the decrease ratio of the objective function between two adjacent iterations is less than a preset threshold, or the norm of the state variable increment is less than a preset threshold, the optimization is determined to be converged.

[0178] Step S8: Based on the optimized unified trajectory results of the two vehicles, provide a pose reference for the unified coordinate transformation, stitching, or incremental map update of the local map or local point cloud data of the two vehicles, including:

[0179] Based on the optimized trajectories of the first and second vehicles, the local point cloud data collected by each vehicle is transformed to a unified world coordinate system for point cloud stitching and fusion, providing a unified pose reference for globally consistent point cloud maps, raster maps, or incremental map updates. At the same time, the optimized modal time-varying reliability state sequences are output as map quality indicators or references for subsequent collaborative tasks.

[0180] Through the above steps, the conversion and output of the optimized trajectory into a globally consistent map are completed, providing the final mapping results and reliability reference information for the dual-vehicle collaborative mapping task.

[0181] Example 2

[0182] This embodiment 2 takes a long corridor degradation scenario as an example. Based on the dual-vehicle heterogeneous collaborative mapping method based on UWB weak global constraints and robust fusion of reliability perception provided in embodiment 1, a simulation experiment is conducted. The method is compared with the existing dual-vehicle trajectory mapping method that relies solely on odometer constraints. The simulation results are as follows:

[0183] Figure 4 This is a comparison chart of dual-vehicle trajectories in a long corridor degradation scenario using existing methods that rely solely on odometer constraints. Figure 4 The image shows the actual trajectory of the first vehicle, the actual trajectory of the second vehicle, the estimated trajectory of the first vehicle, the estimated trajectory of the second vehicle, and the location of UWB anchor points with known coordinates in the environment. The shaded areas indicate time intervals where the first vehicle's odometer is significantly affected by degradation. Figure 4 It is evident that in scenarios with insufficient geometric constraints, such as long corridors, relying solely on odometer constraints can easily lead to cumulative deviations in the estimated trajectory over time, making it difficult to maintain a stable relative relationship between the two vehicle trajectories. Figure 5 These are comparison images of dual-vehicle trajectories in the same long corridor degradation scenario, using the method described in Embodiment 1 of this invention. Figure 5 The image shows the actual trajectory of the first vehicle, the actual trajectory of the second vehicle, the optimized trajectory of the first vehicle, the optimized trajectory of the second vehicle, and the UWB anchor point positions. The shaded area is... Figure 4Correspondingly, they represent the same odometer degradation interval. This embodiment introduces UWB weak global constraints and intra-graph reliability states into the unified back-end factor graph, enabling the odometer constraints and UWB ranging constraints to be adaptively fused based on observation reliability, thereby maintaining good consistency between the dual-vehicle trajectories and the actual trajectories even in long corridor degradation intervals. Figure 6 It corresponds Figure 4 The graph shows the change of the relative error of the two-vehicle trajectory over time during the simulation. The relative error is defined as the Euclidean distance between the estimated relative displacement of the two vehicles and the actual relative displacement of the two vehicles. Figure 6 The shaded area in the image represents the degradation range of the first vehicle's odometer. Figure 6 It is evident that the method relying solely on odometer constraints exhibits significant relative displacement errors between the two vehicles under the influence of long corridor degradation, reflecting that the lack of weak global constraints makes it easy for the unified trajectory of the two vehicles to drift relative to each other. Figure 7 It corresponds Figure 5 The graph shows the relative error of the two vehicle trajectories over time during the simulation. This graph uses the same... Figure 6 The same definition of relative error and the same labeling of degradation intervals. Figure 7 As can be seen, after introducing UWB weak global constraints, diagnostic reliability priors, in-map reliability states, and reliability time smoothing constraints, the relative displacement error between the two vehicles remains within a small range overall, indicating that the embodiments of the present invention can suppress the relative drift between the two vehicles caused by the degradation of a single odometer in long corridor degradation scenarios. Figure 8 This is a schematic diagram comparing the dual-vehicle trajectory with the actual trajectory in a scenario where some UWB anchor points are visible and NLOS interference exists, using the method of Embodiment 1 of the present invention. Figure 8 The document presents the UWB anchor point location, the actual trajectories of the first and second vehicles, the initial trajectory obtained from odometer integration, and the dual-vehicle trajectory after reliability perception joint optimization. Figure 8 This invention illustrates that, under conditions of UWB link quality fluctuations, partial anchor point visibility, and NLOS interference, the method provided in this embodiment can integrate UWB weak global constraints and dual-vehicle odometer constraints into a unified backend optimization framework and output dual-vehicle trajectory estimation results in a unified coordinate system. Figure 9 Corresponding to Figure 8 A schematic diagram showing the change of UWB reliability status over time during the simulation process. Figure 9 This includes diagnostic reliability sequences for the UWB modes of the first and second vehicles, as well as the in-graph UWB reliability state sequence obtained through joint optimization in the second stage. Figure 9 This is used to explain that when NLOS interference or UWB ranging quality deteriorates, the UWB reliability status decreases with the diagnostic results, thereby reducing the impact of abnormal ranging edges on joint optimization; after the ranging quality is restored, the reliability status rises accordingly, allowing normal UWB ranging constraints to participate in trajectory optimization again.

[0184] Example 3

[0185] Based on the same inventive concept, this embodiment 3 provides a dual-vehicle heterogeneous collaborative mapping system based on the fusion of UWB weak global constraints and reliability perception robustness, including a data acquisition module, a factor graph construction module, a first-stage optimization module, a reliability diagnosis module, a reliability state modeling module, a joint optimization module, and an output module.

[0186] The data acquisition module is used to acquire multi-source observation data of the first vehicle and the second vehicle, and to construct a basic observation set for dual-vehicle heterogeneous collaborative mapping. The multi-source observation data includes at least the odometer data of the first vehicle, the odometer data of the second vehicle, UWB anchor point coordinate information, and UWB ranging data.

[0187] The factor graph construction module constructs a unified backend factor graph based on the basic observation set, including a first vehicle odometer factor, a second vehicle odometer factor, a UWB ranging factor, and a pose prior factor.

[0188] The first-stage optimization module is used to perform the first-stage basic optimization on the unified backend factor graph to obtain the initial trajectory estimation results of the two vehicles.

[0189] The reliability diagnosis module is used to construct diagnostic reliability sequences for the first vehicle odometer mode, the second vehicle odometer mode, and the UWB mode based on the initial trajectory estimation results of the two vehicles and the multi-source observation data.

[0190] The reliability state modeling module is used to take the diagnostic reliability sequence as a priori observation, introduce the original reliability state variables in the graph, and constrain the original reliability state variables in the graph within a preset reliability interval through bounded mapping to obtain the bounded reliability state variables in the graph.

[0191] The joint optimization module is used to construct a joint optimization cost function that includes observation residuals, diagnostic reliability priors, and bounded reliability state-time smoothing terms.

[0192] The output module is used to perform a second-stage joint optimization on the trajectory states of the two vehicles and the intra-graph bounded reliability state variables of each mode based on the joint optimization cost function, so as to obtain the optimized unified trajectory results of the two vehicles and the time-varying reliability state sequences of each mode; and, based on the optimized unified trajectory results of the two vehicles, to provide a pose reference for the unified coordinate transformation, stitching or incremental map update of the local map or local point cloud data of the two vehicles.

[0193] Furthermore, this embodiment also includes a weighted balancing module, which is used to increase the weight of the UWB reliability diagnosis prior terms according to the square root of the number of UWB ranging edges at the same time when a single UWB reliability state corresponds to multiple UWB ranging edges.

[0194] It is worth noting that the system embodiment corresponds to the above method embodiment. The implementation methods of the above method embodiments are all applicable to the system embodiment and can achieve the same or similar technical effects, so they will not be described in detail here.

[0195] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0196] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), 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.

[0197] 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.

[0198] 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.

[0199] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the specific implementation of the present invention. Any modifications or equivalent substitutions that do not depart from the spirit and scope of the present invention should be covered within the scope of protection of the claims of the present invention.

Claims

1. A dual-vehicle heterogeneous collaborative mapping method based on the fusion of UWB weak global constraints and reliability-aware robustness, characterized in that, Includes the following steps: Acquire multi-source observation data of the first vehicle and the second vehicle, and construct a basic observation set for dual-vehicle heterogeneous collaborative mapping. The multi-source observation data includes at least the odometer data of the first vehicle, the odometer data of the second vehicle, UWB anchor point coordinate information, and UWB ranging data. A unified backend factor graph is constructed based on the aforementioned basic observation set, and the first stage of basic optimization is performed to obtain the initial trajectory estimation results of the two vehicles. The unified backend factor graph includes a first vehicle odometer factor, a second vehicle odometer factor, a UWB ranging factor, and a pose prior factor. Based on the initial trajectory estimation results of the two vehicles and multi-source observation data, diagnostic reliability sequences for the first vehicle odometer mode, the second vehicle odometer mode, and the UWB mode are constructed respectively, including: The variance of the translational and rotational increments of the vehicle odometer within the sliding window is statistically analyzed. The ratio of the current window variance to the historical baseline variance is used as the diagnostic value of the vehicle odometer mode. The diagnostic value of the vehicle odometer mode is mapped to the diagnostic reliability sequence of the vehicle odometer mode through a dead zone function or a nonlinear decay function. Based on the initial trajectory estimation results of the two vehicles obtained from the first-stage basic optimization, the time step is calculated. The whitened absolute residuals of each UWB ranging edge; the UWB ranging edge refers to the constraint edge formed by a UWB ranging observation in the factor graph; Time The mean and maximum terms of the whitening absolute residuals of each UWB ranging edge are weighted and aggregated to obtain the time. UWB modal diagnostic residuals; The diagnostic value of the robustly normalized UWB mode is calculated based on the median and median absolute deviation of the diagnostic residuals of the UWB modes at all times. The diagnostic quantities of the robustly normalized UWB modes are processed for dead zones and mapped to the original diagnostic reliability of the UWB modes through a preset mapping function. The original diagnostic reliability of the UWB mode is smoothed and limited to obtain the diagnostic reliability of the UWB mode. The diagnostic reliability of the UWB mode at all times constitutes the diagnostic reliability sequence of the UWB mode. The diagnostic reliability sequence is used as a priori observation, and the original reliability state variables within the graph are introduced. These original reliability state variables are then constrained within a preset reliability interval through bounded mapping, resulting in bounded reliability state variables within the graph, including: Based on the diagnostic reliability sequence, the original reliability state variables within the initial graph are initialized as follows: ， ， in, This represents the original reliability state variables within the diagram. Indicates a diagnostic reliability sequence. Indicates will Normalization to Intermediate variables after the interval Indicates the lower limit of reliability. A positive number used for amplitude limiting. This indicates a limit operation. Represent the natural logarithm function; The original reliability state variables in the graph include: the original reliability state variables in the graph for the first vehicle odometer mode, the second vehicle odometer mode, the first vehicle UWB mode, and the second vehicle UWB mode. The original reliability state variables in the graph are mapped to the range from the preset reliability lower limit to 1 by the Sigmoid function, resulting in the bounded reliability state variables in the graph. The bounded reliability state variables in the graph approach the preset reliability lower limit when the original reliability state variables in the graph approach negative infinity, and approach 1 when the original reliability state variables in the graph approach positive infinity. Construct a joint optimization cost function that includes observation residuals, diagnostic reliability priors, and bounded reliability state-time smoothing terms; Based on the joint optimization cost function, the second-stage joint optimization is performed on the dual-vehicle trajectory state and the in-graph bounded reliability state variables of each mode to obtain the optimized dual-vehicle unified trajectory result and the time-varying reliability state sequence of each mode. Based on the optimized unified trajectory results of the two vehicles, a pose reference is provided for the unified coordinate transformation, stitching, or incremental map update of the local map or local point cloud data of the two vehicles.

2. The dual-vehicle heterogeneous collaborative mapping method based on UWB weak global constraints and reliability-aware robust fusion as described in claim 1, characterized in that, The first vehicle odometer data is generated by a lidar odometer or a lidar-inertial odometer; the second vehicle odometer data is generated by a visual odometer, a visual-inertial odometer, or an RGB-D odometer; the UWB anchor point is an absolute reference node pre-deployed in the environment with known coordinates; the UWB ranging data is the time-of-flight ranging value between the UWB tag carried by each vehicle and each UWB anchor point.

3. The dual-vehicle heterogeneous collaborative mapping method based on UWB weak global constraints and reliability-aware robust fusion as described in claim 2, is characterized in that... The basic observation set for constructing the dual-vehicle heterogeneous collaborative mapping includes: Based on a unified hardware clock or interpolation strategy, odometer data and UWB ranging data of different frequencies are aligned to the same time axis, and UWB anchor point coordinate information is used as a static calibration parameter. The anchor point is associated with the corresponding UWB ranging data to form a basic observation set for dual-vehicle heterogeneous collaborative mapping.

4. The dual-vehicle heterogeneous collaborative mapping method based on UWB weak global constraints and reliability-aware robust fusion as described in claim 1, characterized in that, The construction of a unified backend factor graph based on the aforementioned basic observation set includes: The two-dimensional position and yaw angle estimation represent the two-dimensional pose state of the vehicle at a single moment, and the two-dimensional pose increment transformation information represents the relative motion between pose states at adjacent moments. These are used as inputs to the unified back-end factor graph for odometer observations to construct vehicle odometer factors that connect the pose nodes of the vehicle at adjacent moments. The vehicle odometer factors include a first vehicle odometer factor and a second vehicle odometer factor. The two-dimensional pose increment transformation information is represented as follows: ， ， ， ， in Represents two-dimensional pose increment transformation information. and This represents the translational component of the displacement in the vehicle's local coordinate system at adjacent time points. This indicates the change in yaw angle between adjacent time points. Used to normalize the angle to a preset angle range. , , They represent the first The car is at the moment The horizontal and vertical components of the two-dimensional pose state, as well as the yaw angle. , , They represent the first The car is at the moment The horizontal and vertical components of the two-dimensional pose state, as well as the yaw angle. ; The UWB ranging factor connects the vehicle's UWB tag node and the UWB anchor node, and is used to constrain the Euclidean distance between the vehicle's pose state and the known UWB anchor coordinates. Pose prior factor, used to fix the pose of the first frame to eliminate the degree of freedom of the reference frame.

5. The dual-vehicle heterogeneous collaborative mapping method based on UWB weak global constraints and reliability-aware robust fusion as described in claim 4, characterized in that, The first stage of basic optimization yields the initial trajectory estimation results for both vehicles, including: The first stage of basic optimization is performed using the Gauss-Newton method or the Levenberg-Marquardt method. The optimization objective is to minimize the weighted sum of squares of the vehicle odometer factor, UWB ranging factor, and pose prior factor to obtain the initial trajectory estimation results for the two vehicles.

6. The dual-vehicle heterogeneous collaborative mapping method based on UWB weak global constraints and reliability-aware robust fusion as described in claim 5, is characterized in that... The construction of the joint optimization cost function, comprising observation residual terms, diagnostic reliability prior terms, and bounded reliability state-time smoothing terms, includes: ， in, To jointly optimize the cost function, For the set of trajectory states of two vehicles, This represents the original set of reliability state variables within the diagram. For the first The a priori residuals of the vehicle's position and orientation. For the first Information matrix of prior factors for parking position pose. For the first Information matrix of vehicle odometer residuals For the first The car is at the moment Vehicle odometer residuals, For the first The car is at the moment For the Distance residuals of each UWB anchor point The information weights for UWB ranging residuals, For the first The vehicle's UWB ranging observation set, and The first The car is at the moment and time The in-circuit bounded reliability state variables of the odometer mode. and The first The car is at the moment and time The graph-bounded reliability state variables of the UWB mode. and The first The car is at the moment The reliability of odometer diagnostics and the reliability of UWB diagnostics. and These represent the prior weights for diagnostic reliability in the vehicle odometer mode and the UWB mode, respectively. and These are the weights of the bounded reliability state-time smoothing terms for the vehicle odometer mode and the UWB mode, respectively. These are the balanced weighting coefficients for the prior terms of UWB modal reliability diagnosis.

7. The dual-vehicle heterogeneous collaborative mapping method based on UWB weak global constraints and reliability-aware robust fusion as described in claim 6, is characterized in that... The second-stage joint optimization is performed on the dual-vehicle trajectory state and the in-graph bounded reliability state of each mode based on the joint optimization cost function, resulting in the optimized unified trajectory result of the dual vehicles and the time-varying reliability state sequence of each mode, including: The Levenberg-Marquardt method is used to iteratively solve the joint optimization cost function, and the dual-vehicle trajectory state set is updated simultaneously in each iteration. With the original set of reliability variables in the figure The corresponding in-graph bounded reliability state variables are obtained through bounded mapping until the decrease in the joint optimization cost function is less than a preset threshold or the maximum number of iterations is reached. The optimized first vehicle trajectory, second vehicle trajectory and corresponding time-varying reliability state sequence are then output.

8. A dual-vehicle heterogeneous collaborative mapping system based on the fusion of UWB weak global constraints and reliability-aware robustness, characterized in that, The system is used to implement the dual-vehicle heterogeneous collaborative mapping method based on UWB weak global constraints and reliability-aware robust fusion as described in any one of claims 1 to 7, the system comprising: The data acquisition module is used to acquire multi-source observation data of the first vehicle and the second vehicle, and to construct a basic observation set for dual-vehicle heterogeneous collaborative mapping. The multi-source observation data includes at least the odometer data of the first vehicle, the odometer data of the second vehicle, UWB anchor point coordinate information, and UWB ranging data. The factor graph construction module constructs a unified backend factor graph based on the basic observation set, including a first vehicle odometer factor, a second vehicle odometer factor, a UWB ranging factor, and a pose prior factor. The first-stage optimization module is used to perform the first-stage basic optimization on the unified backend factor graph to obtain the initial trajectory estimation results of the two vehicles. The reliability diagnosis module is used to construct diagnostic reliability sequences for the first vehicle odometer mode, the second vehicle odometer mode, and the UWB mode based on the initial trajectory estimation results of the two vehicles and the multi-source observation data. The reliability state modeling module is used to take the diagnostic reliability sequence as a priori observation, introduce the original reliability state variables in the graph, and constrain the original reliability state variables in the graph within a preset reliability interval through bounded mapping to obtain the bounded reliability state variables in the graph. The joint optimization module is used to construct a joint optimization cost function that includes observation residuals, diagnostic reliability priors, and bounded reliability state-time smoothing terms. The output module is used to perform a second-stage joint optimization on the trajectory states of the two vehicles and the intra-graph bounded reliability state variables of each mode based on the joint optimization cost function, so as to obtain the optimized unified trajectory results of the two vehicles and the time-varying reliability state sequences of each mode; and, based on the optimized unified trajectory results of the two vehicles, to provide a pose reference for the unified coordinate transformation, stitching or incremental map update of the local map or local point cloud data of the two vehicles.

Images