Distributed cooperative positioning method based on registration optimization and medium

Abstract

The invention relates to a distributed cooperative positioning method based on registration optimization and a medium, and the method comprises the steps: firstly carrying out the precise calculation of the attitude and speed of each node through employing a quaternion attitude filtering algorithm and a zero-speed correction algorithm, and generating an initial position estimation value; and then, by sharing the initial position estimation and the ranging data of each node, constructing an optimization function with the minimization of the sum of squares of ranging errors as a target, and efficiently solving the nonlinear optimization problem by adopting a sequential least square programming algorithm so as to obtain the corrected translation amount and the rotation angle of each node. Compared with a traditional method depending on an extended Kalman filter, an unscented Kalman filter and a particle filter algorithm, the method does not need to carry out accurate modeling on the system, the requirement for computing resources is remarkably reduced, and meanwhile the robustness for abnormal values and the nonlinear characteristic of the system is enhanced.

Description

Technical Field

[0001] The present invention relates to the technical field of cooperative navigation, and more particularly to a distributed cooperative positioning method and medium based on registration optimization. Background Art

[0002] Cooperative positioning technology has been widely applied in various fields such as modern navigation, aviation, land transportation, and military. This technology realizes the precise determination and tracking of the relative and absolute positions of each vehicle through the sharing and fusion of positioning information among multiple moving vehicles. Traditional multi-moving vehicle positioning systems rely to a large extent on the Global Navigation Satellite System (GNSS), such as GPS, to provide high-precision position information. However, the positioning performance of GNSS significantly degrades in indoor environments, urban canyon areas with high-rise buildings, or in the presence of electromagnetic interference, restricting the application of cooperative positioning systems in more complex and variable scenarios. In addition, the continuous dependence on GNSS exhibits high vulnerability in dynamic and complex environments, further restricting its application scope.

[0003] To address the limitations of GNSS, some traditional ground-based cooperative positioning methods have emerged, such as systems based on WiFi signals and Ultra-Wideband (UWB) anchors. These methods can provide relatively reliable positioning services in specific environments, but they usually rely on pre-deployed infrastructure, resulting in limited application in unknown environments or scenarios requiring rapid deployment, and it is difficult to meet the requirements of flexibility and wide adaptability.

[0004] The Micro Inertial Measurement Unit (MIMU), as an inertial measurement device that is not restricted by the external environment, has become a key component for enhancing the autonomy of positioning systems due to its small size and high autonomy. The MIMU module mainly consists of a three-axis gyroscope and a three-axis accelerometer, and can be additionally equipped with sensors such as a magnetometer and a barometer. By measuring the motion parameters of the vehicle in the inertial space and combining the initial motion state, the attitude and heading of the vehicle can be deduced. However, inertial navigation devices generally suffer from data drift problems, and the errors will accumulate rapidly over time when used alone. Therefore, in practical applications, it is usually necessary to combine magnetometer measurements, virtual zero-speed detection, or other auxiliary positioning information to improve the positioning accuracy.

[0005] The magnetometer uses anisotropic magneto - resistance (AMR) technology to measure the Earth's magnetic field by detecting changes in the magnetic induction intensity in space, thereby obtaining the horizontal northward orientation for correcting the yaw angle of the gyroscope. However, traditional magnetometer - based heading estimation algorithms are vulnerable to magnetic field variations, affecting the positioning accuracy.

[0006] In the architecture design of multi - mobile - vehicle cooperative positioning systems, existing methods are mainly divided into two structures: centralized and distributed. The centralized structure aggregates all positioning information to the central node for unified processing. Although it has advantages in information integration, it has a large computational load and the system stability is vulnerable to the failure of the central node, and it is suitable for scenarios where there are large differences in the information - processing capabilities of mobile vehicles. The distributed structure, on the other hand, allows each node to independently obtain sensor information, share information resources, and perform independent positioning calculations. Compared with the centralized method, it has lower information transmission and computational complexity, and has advantages such as small computational load, strong robustness, less susceptibility to interference, and good scalability, making it more suitable for applications in dynamic and complex environments. Therefore, the distributed structure has more advantages in the cooperative positioning problem based on mutual ranging.

[0007] Existing distributed cooperative positioning methods mostly adopt filtering algorithms such as the Extended Kalman Filter (EKF), Unscented Kalman Filter (UKF), and particle filter algorithm. Although these filtering methods can effectively fuse multi - source data, they often require accurate system modeling when dealing with nonlinear systems, or consume a large amount of computational resources, posing great challenges to practical applications. In addition, the sensitivity of filtering algorithms to outliers and the nonlinear characteristics of the system also limit their performance. Summary of the Invention

[0008] The present invention provides a distributed cooperative positioning method and medium based on registration optimization, aiming to solve the problems of low computational efficiency and insufficient positioning accuracy in dealing with nonlinear and abnormal data in traditional distributed cooperative positioning methods in dynamic and complex environments.

[0009] To achieve the above - mentioned purpose, the first aspect of the present invention provides a distributed cooperative positioning method based on registration optimization, including the following steps:

[0010] Obtain the attitude data of each node, and use the quaternion attitude filtering algorithm to solve the attitude data;

[0011] Combine the results of the attitude data solution with the speed and acceleration information of inertial navigation, limit the speed through the zero - velocity correction algorithm, and calculate the preliminary position estimation value of the node;

[0012] Obtain the ranging data between nodes and share the preliminary position estimates of the nodes;

[0013] Based on the ranging data between nodes and the preliminary position estimates, with the translation amount and rotation angle as optimization variables, establish an optimization objective function aiming at minimizing the sum of squared errors of the ranging data;

[0014] Use the sequential least squares programming algorithm to solve the optimization objective function, and calculate the corrected translation amount and rotation angle of each node;

[0015] According to the optimization solution results, correct the preliminary position estimates of each node to complete the cooperative positioning between nodes.

[0016] Furthermore, the method for resolving the attitude data using the quaternion attitude filtering algorithm includes:

[0017] Obtain the attitude data collected by multi-node sensors, where the attitude data includes three-axis acceleration, three-axis angular velocity, and three-axis geomagnetic intensity;

[0018] Based on the attitude data, perform 6D attitude estimation and 9D attitude estimation respectively using the quaternion attitude filtering algorithm, where:

[0019] The 6D attitude estimation is calculated based on the data of three-axis angular velocity and three-axis acceleration;

[0020] The 9D attitude estimation is calculated based on the data of three-axis angular velocity, three-axis acceleration, and three-axis geomagnetic intensity;

[0021] During the attitude estimation process, decouple and execute the 6D and 9D estimation steps in parallel;

[0022] Represent the attitude data obtained by decoupled parallel processing using quaternions to complete the resolution of the attitude of each node.

[0023] Furthermore, the method for obtaining the attitude data of each node includes: collecting the data of three-axis gyroscopes, three-axis accelerometers, and three-axis magnetometers through the micro-inertial measurement unit carried on the node, and combining the fixed connection method between the micro-inertial measurement unit and the node's foot to obtain the original data reflecting the motion attitude of the node.

[0024] Furthermore, the method for combining the results of attitude data resolution with the speed and acceleration information of inertial navigation, restricting the speed through the zero-velocity correction algorithm, and calculating the preliminary position estimate of the node includes:

[0025] Based on the results of attitude resolution, project the acceleration collected by inertial navigation onto the navigation coordinate system and compensate for the gravitational acceleration to obtain the net acceleration in the local geographical coordinate system;

[0026] Integrate the net acceleration once to calculate the velocity;

[0027] Use the improved generalized likelihood ratio test method to detect the zero-velocity state, including the following steps:

[0028] a) Perform low-pass filtering on the triaxial acceleration and triaxial angular velocity data within the sampling window respectively to obtain the filtered acceleration and angular velocity data;

[0029] b) Based on the filtered acceleration data, calculate its deviation from the gravitational acceleration in the navigation coordinate system, and take the mean and standard deviation of this deviation as the preliminary characteristics of the stationary state;

[0030] c) Based on the filtered angular velocity data, calculate its mean and standard deviation as the supplementary characteristics of the stationary state;

[0031] d) Construct a generalized likelihood ratio test statistic, and combine the preliminary characteristics of the stationary state and the supplementary characteristics of the stationary state into a comprehensive judgment index;

[0032] e) On the basis of the traditional generalized likelihood ratio test method, introduce gait phase persistence detection, logically associate the comprehensive judgment indexes of multiple consecutive detection windows, and judge the zero-velocity state;

[0033] When the zero-velocity state is detected, set the velocity of this period to zero, and calculate the acceleration drift amount according to the velocity change within the zero-velocity stage;

[0034] Compensate the detected acceleration drift amount and correct the velocity to obtain the corrected velocity;

[0035] Perform a second integration on the corrected velocity to calculate the preliminary position estimate value of the node.

[0036] Furthermore, the method for obtaining the ranging data between nodes and sharing the preliminary position estimate values of nodes includes:

[0037] Each node collects the ranging data between each other through UWB signals;

[0038] Each node transmits the preliminary position estimate value calculated by inertial navigation by itself to other nodes through UWB signals, and at the same time receives the preliminary position estimate values of other nodes;

[0039] Associate and store the ranging data at the current moment with the preliminary position estimate values shared between nodes.

[0040] Furthermore, the method for establishing the optimization objective function includes:

[0041] Based on the ranging data between nodes, calculate the distance error between each pair of nodes;

[0042] Using the preliminary position estimation values of the nodes, determine the relative position differences between each pair of nodes;

[0043] Taking the distance error and relative position difference between the nodes as input variables, establish an optimization objective function, and determine the optimization variables of the objective function. Among them, the objective function aims to minimize the sum of the squared deviations between the ranging data between the nodes and the relative position differences.

[0044] Furthermore, the method for solving the optimization objective function and calculating the corrected translation amounts and rotation angles of each node includes:

[0045] According to the established optimization objective function, initialize the initial values of the translation amount and rotation angle;

[0046] Use the sequential least squares programming algorithm to iteratively solve the optimization objective function;

[0047] In each iteration, calculate the objective function value and its gradient corresponding to the current translation amount and rotation angle;

[0048] Update the translation amount and rotation angle according to the gradient information until the objective function value converges or reaches the preset number of iterations;

[0049] Output the corrected translation amounts and rotation angles obtained by the optimization solution.

[0050] Furthermore, the correction method based on the optimization solution also includes the mutual registration strategy between the nodes. The mutual registration strategy includes:

[0051] After each ranging, record the ranging node and its corresponding ranging data, and dynamically update the cumulative distance of the registration segment of each node relative to the current reference node;

[0052] When the cumulative distance of the registration segment of the current reference node reaches the preset threshold or meets the optimization condition, select the node with the longest cumulative distance as the new reference node;

[0053] Notify all nodes to update the reference node, and at the same time clear their respective cumulative registration segment data and redefine the optimization objective function;

[0054] Perform optimization calculations on the ranging data within the updated registration segment.

[0055] Furthermore, the mutual registration strategy introduces the following constraint conditions during the optimization process:

[0056] The translation amount and rotation angle within each registration segment need to satisfy that the rotation angle does not exceed the preset maximum value and the translation amount does not exceed the set threshold;

[0057] During the optimization solution process, exclude the outliers in the ranging data within the registration segment;

[0058] Dynamically update and optimize the weight parameters of the objective function.

[0059] To achieve the above object, a second aspect of the present invention provides a computer-readable storage medium, on which a computer program is stored, and when the computer program is run by a processor, it executes the steps of the fake news detection method based on multimodal information fusion.

[0060] Advantages of the present invention:

[0061] Compared with the prior art, a distributed cooperative positioning method and medium based on registration optimization provided by the present invention effectively overcome the problems of low computational efficiency and insufficient positioning accuracy faced by traditional filtering algorithms when dealing with non-linear and abnormal data in a dynamic and complex environment by transforming the position correction problem into a combinatorial optimization problem. Specifically, this method first uses the quaternion attitude filtering algorithm and the zero-velocity correction algorithm to accurately calculate the attitude and velocity of each node, generating a preliminary position estimate. Subsequently, by sharing the preliminary position estimates and ranging data of each node, an optimization function with the goal of minimizing the sum of squared ranging errors is constructed, and the sequential least squares programming (SLSQP) algorithm is used to efficiently solve this non-linear optimization problem, thereby obtaining the corrected translation amount and rotation angle of each node. Compared with the methods that traditionally rely on the extended Kalman filter (EKF), unscented Kalman filter (UKF), and particle filtering algorithms, the method of the present invention does not require accurate modeling of the system, significantly reduces the demand for computing resources, and at the same time enhances the robustness to outliers and the non-linear characteristics of the system. Through this optimization solution strategy, it is possible to significantly improve the accuracy and reliability of multi-mobile vehicle cooperative positioning while ensuring real-time performance, and it is particularly suitable for dynamic and complex application environments, thus effectively solving the limitations of existing distributed cooperative positioning methods in complex scenarios. Description of the Drawings

[0062] To more clearly illustrate the technical solutions in the embodiments of the present invention, the following will briefly introduce the drawings required for the description of the embodiments.

[0063] Figure 1 It is a framework of a distributed cooperative positioning system based on registration optimization disclosed in an embodiment of the present invention.

[0064] Figure 2 It is a framework of a single-node independent inference algorithm disclosed in an embodiment of the present invention.

[0065] Figure 3 It is a flowchart of a distributed cooperative positioning method based on registration optimization disclosed in an embodiment of the present invention.

[0066] Figure 4 It is the difference between the VQF filter and traditional filtering algorithms disclosed in an embodiment of the present invention.

[0067] Figure 5 It is a ZUPT algorithm framework disclosed in an embodiment of the present invention.

[0068] Figure 6 It is a schematic diagram of registration optimization disclosed in an embodiment of the present invention.

[0069] Figure 7 It is a flowchart of a registration algorithm disclosed in an embodiment of the present invention.

[0070] Figure 8 It is a physical diagram of a single node disclosed in an embodiment of the present invention.

[0071] Figure 9 It is a hardware structure diagram disclosed in an embodiment of the present invention.

[0072] Figure 10 It is a comparison chart of speeds before and after zero speed correction disclosed in an embodiment of the present invention.

[0073] Figure 11 It is a true trajectory diagram of Experiment 1 disclosed in an embodiment of the present invention.

[0074] Figure 12 It is a positioning result diagram of Experiment 1 disclosed in an embodiment of the present invention.

[0075] Figure 13 It is a true trajectory diagram of Experiment 2 disclosed in an embodiment of the present invention.

[0076] Figure 14 It is a positioning result diagram of Experiment 2 disclosed in an embodiment of the present invention. Detailed implementation manners

[0077] In order to enable those skilled in the art to better understand the solution of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0078] The present invention proposes a distributed cooperative positioning method based on registration optimization, which can effectively improve the positioning accuracy without relying on external base stations, and the model establishment process is simple and clear, and has great advantages compared with traditional filtering algorithms in dealing with the problem of cooperative positioning. Its system frame design is as Figure 1As shown in the figure, it is assumed that the system consists of N members, where the i-th member is represented as Pedestriani, and i = 1, 2, 3,... N. For simplicity, unless otherwise stated, Pedestriani will be discussed in the following content. For this system, the sensors carried by each member include a MIMU module (which includes a gyroscope, an accelerometer, and a magnetometer) and UWB. During the walking process, each member collects the outputs of its own gyroscope, accelerometer, and magnetometer, combines the zero-velocity correction algorithm to calculate its own position information, and then transmits the preliminary position information to other nodes through UWB signals. At the same time, it receives the position information of other nodes and the measured distances. After reaching the registration condition with the current reference node, it performs registration optimization to correct its own position and yaw angle, and then obtains more accurate position information. The key to the proposed method lies in using optimized registration for multi-node information fusion, and single-node independent calculation is the basis for multi-node collaborative positioning.

[0079] The single-node displacement estimation algorithm framework designed by the present invention is as Figure 2 shown. First, the VQF algorithm is used to fuse the sensor data to obtain the sensor attitude, and further project the acceleration and angular velocity of the carrier onto the NED navigation coordinate system. On the one hand, the projected acceleration and angular velocity can be used to judge whether the gait is in the grounded stationary state. On the other hand, the velocity can be estimated by integrating the node attitude and the projected acceleration, and the velocity can be corrected according to the zero-velocity state. Finally, the corrected velocity is integrated to obtain the displacement estimation value of the node.

[0080] The following will expand and explain from the positioning method. It should be noted that the steps shown in the flowchart of the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions. And although the logical order is shown in the following method, in some cases, the steps shown or described can be executed in a different order than here.

[0081] As Figure 3 shown, the present invention provides a distributed collaborative positioning method based on registration optimization, including the following steps:

[0082] Step S100: Obtain the attitude data of each node, and use the quaternion attitude filtering algorithm to calculate the attitude data;

[0083] Step S200: Combine the result of the attitude data calculation with the speed and acceleration information of inertial navigation, limit the speed through the zero-velocity correction algorithm, and calculate the preliminary position estimation value of the node;

[0084] Step S300: Obtain the ranging data between each node, and share the preliminary position estimation value of the node;

[0085] Step S400: Based on the ranging data between nodes and the preliminary position estimation values, with the translation amount and rotation angle as optimization variables, establish an optimization objective function aiming to minimize the sum of squares of ranging data errors.

[0086] Step S500: Use the sequential least squares programming algorithm to solve the optimization objective function, and calculate the corrected translation amount and rotation angle of each node.

[0087] Step S600: Correct the preliminary position estimation values of each node according to the optimization solution results to complete the collaborative positioning between nodes.

[0088] In this embodiment, as described in step S100 above, each node first collects attitude data through its carried micro-inertial measurement unit (MIMU), including three-axis acceleration, three-axis angular velocity, and three-axis geomagnetic intensity. These data are measured in real time by the accelerometer, gyroscope, and magnetometer respectively, aiming to comprehensively reflect the motion attitude of the node in three-dimensional space.

[0089] To ensure the accuracy and robustness of attitude solution, the Versatile Quaternion-based Filter (VQF) algorithm is adopted. The core of this algorithm lies in the decoupled parallel processing of 6D attitude estimation and 9D attitude estimation to effectively cope with the possible magnetic field interference problems in the environment.

[0090] Wherein:

[0091] 6D attitude estimation: Based on the data of three-axis angular velocity and three-axis acceleration for fusion calculation to generate an attitude solution result not interfered by magnetometer data.

[0092] 9D attitude estimation: Integrate the data of three-axis angular velocity, three-axis acceleration, and three-axis geomagnetic intensity to further improve the absolute accuracy of attitude solution.

[0093] In the actual solution process, the estimation steps of 6D and 9D are executed independently and in parallel, and the attitude results are uniformly output in the form of quaternions. The advantage of quaternions is to avoid the gimbal lock problem existing in the Euler angle representation and can perform attitude interpolation and combination operations more efficiently. The flow framework of attitude solution is as Figure 4 shown. Compared with the traditional method, the VQF algorithm improves the solution accuracy in a magnetic interference environment through parallel processing.

[0094] In this embodiment, as described in step S200 above, the node obtains speed and acceleration information through the inertial navigation system, and combines with the attitude data solved in step S100 to calculate the motion trajectory of the node in detail. Its core objective is to limit the divergence of speed and generate accurate preliminary position estimation values of the node.

[0095] First, using the attitude data calculated in step S100, project the three-axis acceleration collected by inertial navigation into the navigation coordinate system. This process includes compensating for the gravity component in the acceleration to obtain the net acceleration a T . Based on this net acceleration, calculate the velocity estimate through a single integration, with the formula as follows:

[0096]

[0097] where a T is the net acceleration, that is, the acceleration measured by the sensor, is the true acceleration, and Δa T is the change in the acceleration zero bias, which reflects the systematic error in acceleration measurement caused by the drift or change of the zero bias.

[0098] Based on the net acceleration a T , obtain the node velocity estimate v T through integration. The specific calculation formula is:

[0099]

[0100] where v T is the velocity (i.e., the velocity estimate), v T-1 is the velocity at the previous moment, d t is the sampling period, is the transformation matrix from the body coordinate system to the navigation coordinate system at time T, and the acceleration white noise is ignored here.

[0101] Due to the problem of acceleration drift in inertial navigation devices, the cumulative error in velocity calculation is likely to cause large velocity divergence. To solve this problem, this method uses the Zero Velocity Update (ZUPT) algorithm to limit the velocity. The core of zero velocity correction lies in correcting the velocity drift by detecting the gait stance phase (i.e., the foot stationary phase). During the stationary phase, the velocity is set to zero, and the zero bias of the acceleration is estimated based on the velocity change characteristics of this phase.

[0102] The zero velocity state detection uses an improved Generalized Likelihood Ratio Test (GLRT). Its specific process includes:

[0103] Step S201: Perform low-pass filtering on the three-axis acceleration and angular velocity data within the sampling window to obtain the smoothed data;

[0104] Step S202: Calculate the deviation between the acceleration and the gravitational acceleration, and use its mean and standard deviation as the preliminary characteristics of the stationary state. At the same time, calculate the mean and standard deviation of the angular velocity as supplementary characteristics;

[0105] Step S203: Construct the detection statistic λ k , and the formula is as follows:

[0106]

[0107] where λ k is the detection statistic used to determine whether the current time window belongs to the standing state. M is the size of the detection window, representing the width of the time window selected for statistical calculation. k + M represents the end point of the time window, offsetting M time steps into the future, and k - M represents the start point of the time window, offsetting M time steps into the past. j represents a specific time step index within this time window range. σ a is the standard deviation of the specific force measurement value, is the specific force measurement value, the acceleration after gravity compensation, and g is the gravitational acceleration constant. is the average specific force within the time period [k - M, k + M], and σ ω is the standard deviation of the angular velocity measurement value, is the angular velocity measurement value.

[0108] Step S204: When the detection statistic λ k is less than the threshold T λ , it is determined to be in the zero-velocity state, and the output result is as follows:

[0109]

[0110] where D k represents the standing state of the detection window: 1 for stationary and 0 for non-stationary. Combining this detection result with the gait phase persistence mechanism and logically correlating the output values of consecutive detection windows can effectively reduce the false detection and missed detection probabilities during the detection process.

[0111] The cumulative error in inertial navigation mainly stems from acceleration drift. After detecting the zero-velocity state, the acceleration drift Δa T can be estimated and compensated based on the following assumptions:

[0112] Assume that the acceleration drift is a constant value Δa T during a single-step motion process, and it can be estimated using the velocity information in the zero-velocity state. Let a certain step take off from time T and land at T + k, experiencing k sampling periods:

[0113]

[0114] where v T+k represents the velocity at time T + k (in the navigation coordinate system), is the rotation matrix from the body coordinate system (b) to the navigation coordinate system (n), describing the attitude information at time T + k, and aT+k is the true acceleration at time T + k (in the navigation coordinate system), which consists of and Δa T+k . is the ideal value of the acceleration (bias-free value), and Δa T+k is the deviation or zero bias of the acceleration (in the body coordinate system); is the transformation matrix from the inertial coordinate system to the navigation coordinate system; Δa T+m represents the zero bias error or deviation value of the acceleration at time T + m in the body coordinate system, represents the ideal acceleration value at time T + m in the body coordinate system (i.e., the true acceleration without error), k represents the number of sampling points in the current gait, the time range is from 1 to k, and m represents the index of the sampling point.

[0115] Since the true velocities at times T and T + k are both 0, the true velocity increment from T to T + k is:

[0116]

[0117] Therefore, we have:

[0118]

[0119] According to the assumption that the acceleration zero bias is constant during a single-step motion, we have:

[0120]

[0121] That is:

[0122]

[0123] Among them, is the estimated acceleration drift, that is, the estimated value of the zero bias error of the acceleration sensor, d t is the sampling time interval, is the attitude transformation matrix from the inertial coordinate system to the navigation coordinate system, k represents the number of sampling points in the current gait, the time range is from 1 to k, m represents the index of the sampling point, and Δa T is the acceleration zero bias, v T+k and v T are the velocities at the end and start times of the gait respectively;

[0124] Then the velocity estimate value after zero velocity correction at any time is:

[0125]

[0126] Among them, is the estimated velocity at any time T+ΔT after zero-velocity correction. ΔT is the total number of sampling points within the time span from time T to time T+ΔT, and a T+m is the acceleration data measured at time T+m. This process effectively suppresses the growth of cumulative errors in inertial navigation through precise estimation and compensation of the acceleration drift.

[0127] Based on the corrected velocity, the preliminary position estimate of the node is calculated through double integration, and the formula is as follows:

[0128]

[0129] where Δl T+ΔT is the preliminary position estimate of the corrected single node at time T+ΔT. Finally, combined with the velocity data after zero-velocity correction, the node can achieve high-precision independent position calculation.

[0130] Since the zero-velocity correction process dynamically adjusts the velocity estimation within each gait step, the preliminary position estimate can more accurately reflect the movement trajectory of the node within each sampling period. The ZUPT algorithm resets the velocity error during each standing period through the virtual zero-velocity hypothesis, effectively limiting the drift divergence problem of low-cost sensors. The corrected velocity is used for further position calculation, combined with the high-frequency sampling of inertial navigation data, to ensure that the preliminary position estimate has high accuracy. Through real-time zero-velocity detection and drift compensation, it can adapt to the diverse movement patterns of the node in a dynamic environment.

[0131] Figure 5 shows the overall framework of the zero-velocity correction algorithm, clarifying the logical process from zero-velocity state detection to velocity correction and then to position estimation. By utilizing the zero-velocity information in the gait movement of the node, the algorithm effectively limits the cumulative error in inertial navigation, laying a precise preliminary position foundation for cooperative positioning among nodes.

[0132] In this embodiment, as described in the above step S300, there is a problem of unobservable heading angle when using zero-velocity correction in the process of inertial navigation pedestrian position calculation. Therefore, it is necessary to introduce other information to improve the positioning accuracy. This method further improves the accuracy of multi-person cooperative positioning by introducing mutual ranging among members. The idea is as follows:

[0133] Multi-node cooperative positioning is based on the single node completing its own pose independent calculation. Specifically, at time k, after Pedestrian i completes independent calculation, if UWB ranging is performed with the reference node Pedestrianj at the current time, then the distance between the two members is obtained through ranging and the position estimate value transmitted by Pedestrianj through the ranging signal at this time.At the same time, send the position estimated at this moment through the ranging signal

[0134] After each ranging is completed, Pedestriani obtains a distance value and the position estimate of the member that performed ranging with it at this time. When the conditions for optimal registration are met, optimal registration is performed. The core of this process lies in establishing an optimization objective function by introducing the ranging data between nodes and the relative position differences, and continuously adjusting the relative positions and postures between nodes through the optimization process, thereby reducing the overall positioning error.

[0135] To achieve this goal, first obtain the ranging data between each node and share the preliminary position estimates of the nodes. Each node collects the ranging data with each other through UWB signals. At the same time, each node transmits the preliminary position estimate calculated by inertial navigation to other nodes and receives the preliminary position estimates of other nodes. In this way, the preliminary position estimates and ranging data between nodes are dynamically updated and correlated, providing data support for the subsequent optimization process.

[0136] In this embodiment, as described in step S400 above, the establishment of the optimization objective function is as follows: Based on the ranging data between nodes, calculate the distance error between each pair of nodes, and use the preliminary position estimates of the nodes to determine the relative position differences between each pair of nodes. The optimization variable of the objective function is the sum of the squared deviations between the ranging data between nodes and the relative position differences, and the optimization objective is to minimize this deviation.

[0137] When using inertial navigation for solution, the main source of positioning error is the cumulative error caused by the zero biases of the gyroscope and accelerometer. Under the condition that the ZUPT algorithm restricts the divergence of its speed magnitude, it can be considered that its relative displacement within a short period is relatively accurate, and the error comes from the previous position and heading estimates. Therefore, only rotation and translation of this displacement are required to obtain a more accurate trajectory. In the actual positioning process, the true trajectory cannot be obtained, and the mutual ranging based on UWB provides a reference for determining the rotation and translation amounts.

[0138] This method compares the distance between the positions deduced from the ranging points after rotation and translation with the distance measured by UWB, and minimizes the difference between the two by optimizing the rotation angle dθ and the offsets in the horizontal and vertical directions (dx, dy), thereby transforming the position correction problem into an optimization problem. Figure 6 It is a schematic diagram of the above process, where M is the total number of registration points in the current registration segment.

[0139] To solve the optimization problem, it is first necessary to determine the objective function. During the process of multi-member collaborative navigation, the registration of the trajectory needs to be carried out in segments. The larger the error of a trajectory segment, the relatively larger the required trajectory offset (including translation and rotation) during registration. Therefore, it is necessary to determine the segmentation criterion based on the magnitude of the error.

[0140] After registering one of the segments, if the error can be continuously controlled within a certain range, there is no need for re-registration. During the actual movement process, the true value of the trajectory is unknown, so the magnitude of the solution error cannot be directly obtained. This method uses the difference between the calculated spacing value and the spacing value measured by UWB as the basis for judging whether the registration segment division criterion is met. That is:

[0141]

[0142] Among them, and represent the predicted position information of nodes i and j at time k, where k t |k t -1 indicates that these positions are deduced based on the estimation results at the previous time (i.e., k t -1), represents the actual distance obtained by UWB ranging between nodes i and j at time k t . P is the node position, d is the distance measured by UWB, and Δd 0 is the preset error threshold; the function of this formula is to compare the difference between the estimated distance between nodes and the actual ranging value. If the difference is greater than the threshold Δd 0 , it indicates that position correction or optimization is required.

[0143] During the actual registration process, if the registration segment contains too few points, the registration effect will deteriorate. Therefore, a distance limit must be added as the registration segment division criterion. Specifically, the distance of the registration segment must be greater than a preset threshold d 0 , that is:

[0144]

[0145] When both of the above conditions are met, registration optimization begins.

[0146] Next, assume that the initial position of the optimization segment Pedestriani is (x 0 , y 0 ), and the displacement calculated by its own MIMU module at time k is (Δx k , Δy k ). Then, from the initial time k 0 of the registration segment to the ranging time k t , the displacement calculated by its own MIMU module is:

[0147]

[0148] Among them, Δx k and Δy k respectively represent the changes in the displacement from time k - 1 to time k in the horizontal and vertical directions. k 0 is the starting time when registration begins, and k t is the current time, representing the end time of the registration segment. l x and l y respectively represent the cumulative displacements of Pedestriani in the horizontal and vertical directions within the registration segment. It can be understood that based on the displacements at each time calculated by the self-body sensing module (MIMU), the total displacement (in the horizontal and vertical directions) of Pedestriani is from the starting time k 0 of registration to the ranging time k t which is the cumulative sum of all displacement changes.

[0149] The coordinates obtained after rotating this segment of the trajectory by dθ around the initial position and translating by (dx, dy) are as follows:

[0150]

[0151] Among them, (x 0 , y 0 ) is the initial position, dx and dy are the translation amounts calculated according to the MIMU module in the horizontal and vertical directions, and dθ is the rotation angle, that is, the angle by which the trajectory rotates during registration; it can be understood that through the given initial position, according to the translation amount and rotation angle, the current position of Pedestriani is updated.

[0152] For each optimization segment, record in real time the time k when ranging occurs with Pedestrianj (j = 1, 2,... i - 1, i + 1,... N) within this segment, and the corresponding UWB ranging value The position of Pedestrianj Then the difference between the calculated point distance and the UWB ranging can be expressed as:

[0153]

[0154] Among them, is the difference between the calculated position and the distance measured by UWB, is the distance measured by UWB from Pedestrianj to Pedestriani. It can be understood from this formula that by minimizing this difference, the positioning accuracy can be improved.

[0155] Within a registration optimization segment, sum up the above differences of multiple ranging points, and let k r be the set of all points to be registered within the registration segment. Then the total distance difference to be optimized can be expressed as:

[0156]

[0157] By optimizing and solving the parameters for ΔR, that is, by minimizing the following objective function:

[0158]

[0159] where ΔR is the total distance difference of all ranging points, representing the total difference to be optimized within the entire registration segment, and Δr k is the distance difference of a certain ranging point at time k, which calculates the difference between the calculated position and the actual ranging value.

[0160] Thus, the corrected translation amount and rotation angle of each node are obtained. Acting the obtained results on the position calculated by a single node can optimize the solution node position. Among them, the displacement predicted at each step (Δx k , Δy k ) is iterated from its own inertial navigation after zero velocity correction, the measured distance value comes from the UWB signal, and the position information of other nodes at the current moment comes from other nodes. (Sent over by the UWB signal), and the algorithm flow of this part is as Figure 7 shown.

[0161] In this embodiment, as described in the above steps S500 and S600, the objective function is established. Next is the process of solving it. During the optimization and solution process, to avoid the appearance of outliers and consider the rationality of registration, it is necessary to limit the rotation angle and the translation amount. Specifically, there are the following constraint conditions:

[0162] |dθ| < dθ 0 , |dx| < dx 0 , |dy| < dy 0

[0163] During the actual solution process, this method sets dθ 0 , dx 0 , dy 0 to π / 4, 1, 1 respectively, and solves the following non - linear optimization problem:

[0164]

[0165] At the same time, apply the constraint conditions:

[0166]

[0167] This method uses the Sequential Least Squares Programming algorithm (SLSQP) to solve the above problem. This algorithm is suitable for solving non-linear constrained optimization problems. By iteratively solving and generating a search direction for the least squares problem, the optimal solution can be quickly determined, thus improving the solution efficiency. In each iteration, the objective function value and its gradient corresponding to the current translation amount and rotation angle are calculated, and the translation amount and rotation angle are updated until the objective function value converges.

[0168] For the non-linear optimization problem:

[0169]

[0170] where f(x) is the objective function, representing the function to be minimized, x ∈ R n is the decision variable vector, 5 represents the variables of the optimization problem, h i (x) = 0 is the equality constraint function, representing the equality constraint in the optimization problem

[0171] condition, g j (x) ≥ 0 is the inequality constraint function, representing the inequality constraint condition in the optimization problem, f: R n → R is the mapping of the objective function; h i : R n → R is the mapping of the equality constraint function; g i : R n → R is the mapping of the inequality constraint function.

[0172] By iteratively solving and transforming each iteration step into solving the following least squares problem:

[0173]

[0174] where d is the search direction; B k is the approximation of the Hessian matrix of the objective function at the current iteration point x k ; is the gradient of the objective function at x ; k is the gradient of the equality constraint at x ; k is the gradient of the inequality constraint at x

[0175] ; k is the gradient of the inequality constraint at x. In this way, the search direction can be quickly determined,

[0176] and the solution process can be accelerated.

[0177] It should be noted that in the actual positioning process, since the true trajectory cannot be obtained as a reference, it is not possible to simply rely on a single node as a reference node. Instead, mutual registration between multiple nodes is required to reduce the overall positioning error. For this purpose, the present method designs a mutual registration strategy. Specifically, after each ranging, the ranging node and its corresponding ranging data are recorded, and the cumulative distance of the registration segment of each node relative to the current reference node is dynamically updated. When the cumulative distance of the registration segment of the current reference node reaches a preset threshold or meets the optimization condition, the node with the longest cumulative distance is selected as the new reference node. The selection process can

[0178] be carried out in the following way:

[0179]

[0180] where j′ is the currently selected reference node, d i is the distance between node i and the current reference node j, and N is the total number of nodes. It can be understood that when selecting the reference node, the distances between each node and the current reference node are compared, and the node farthest from the current reference node is selected as the new reference node. In this way, the optimization degree of the registration segment can be maximized.

[0181] Then all nodes are notified to update the reference node, and at the same time, the cumulative data of their respective registration segments is cleared, and the optimization objective function is redefined. In this way, the mutual registration between nodes is successfully completed.

[0182] During the optimization process, the translation amount and rotation angle within the registration segment need to meet the following constraint conditions:

[0183] 1. The translation amount and rotation angle within each registration segment do not exceed the preset maximum value;

[0184] 2. During the optimization process, outliers in the ranging data within the registration segment need to be excluded;

[0185] 3. Dynamically update the weight parameters of the optimization objective function.

[0186] Through this mutual registration strategy, the overall positioning accuracy can be significantly improved, and the rationality and effectiveness of the optimization process can be ensured.

[0187] In order to reduce costs and improve portability, the present invention integrates MIMU and UWB. And to ensure the existence of zero speed, the integrated device is fixed to the ankle of the experimental personnel. The physical diagram of a single node and its PCB are as Figure 8 shown.

[0188] The system hardware design is as follows: Using ESP32-S3R8 as the main control chip, externally connect the W25Q128JVSIQ flash memory chip through the eight-wire SPI, and connect and communicate with the functional modules QMC5883 three-axis magnetic sensor, MPU9250 nine-axis gyroscope, MS5611 barometric altitude sensor, and DW1000 ultra-wideband positioning chip through the IIC bus and SPI bus. The hardware structure of the device is as Figure 9 shown.

[0189] The following specific steps take only a single person as an example. The specific implementation manner of the multi-person collaborative positioning algorithm based on registration optimization is as follows:

[0190] First, initialize the optimized trajectory list to store the node positions and trajectories after each optimization. Then, initialize the cumulative distance of registration and prepare the list of registration time intervals. At the same time, set the list of registration reference node positions and set the threshold for distance determination. The moment of the previous registration segment also needs to be initialized to ensure that the registration process can start from the correct time point.

[0191] Enter the main loop and loop until all optimization processes are completed. At the beginning of each loop, update the current time to continue advancing the calculation. Obtain the speed, acceleration, and magnetometer data at the current time, and solve the displacement of the current node through the VQF algorithm. Then, use the inertial correction algorithm to predict the displacement and position of the current node.

[0192] Predict the estimated position of the current node based on the node position at the previous moment and the displacement at the current moment. If there is a valid UWB ranging signal, calculate the difference between the actually measured distance and the predicted position.

[0193] When the difference between the measured distance and the predicted position exceeds the threshold and the distance is greater than the set minimum threshold, start the registration optimization. At this time, add the predicted position at the current time to the optimized trajectory list and update the trajectory according to the registration optimization result. Subsequently, clear the registration data of the optimized nodes and prepare for the next update.

[0194] If the current node is a reference node and other nodes have completed registration, select the node farthest from the current reference node as the new reference node. When updating the reference node, ensure that the selected node can improve the overall optimization effect.

[0195] When all nodes have completed registration and the optimization process converges, the algorithm terminates. At this time, the trajectories of all nodes have been optimized and the expected accuracy has been achieved.

[0196] To further illustrate the above method, the present invention verifies the proposed positioning effect through experiments in a real scenario. The experiment is divided into two main parts, namely the zero-speed correction effect and the collaborative positioning experiment:

[0197] 1. Zero-speed correction effect:

[0198] In this part of the experiment, the present invention verified the effectiveness of the zero-speed correction algorithm by intercepting the zero-speed detection and speed change conditions within a short time period of a single wearable device. Figure 10 The comparison of speeds before and after correction is shown. Among them Figure 10 (a) shows the speed before correction, projected onto the local geographic coordinate system; Figure 10 (b) is the speed after correction, and the red line segment represents the detected standing period. It can be seen from the figure that before correction, the speed error continuously accumulates and diverges, while after correction, the speed during the standing period is set to zero, and the speeds in other periods are compensated, showing periodic changes, and the speed gradually converges, conforming to the speed changes during actual walking. The experimental results prove that the zero-speed correction algorithm effectively limits the divergence of speed and improves the positioning accuracy.

[0199] 2. Cooperative positioning experiment:

[0200] In order to verify the effectiveness and practical application value of the registration-based cooperative positioning algorithm, the present invention conducted a series of experiments in a real scenario, setting different experimental conditions to test the performance of the algorithm in different situations:

[0201] Long-distance walking positioning experiment (Experiment 1): In the experiment, three experimenters started from different starting positions, walked along the same trajectory, and returned to the starting point. The walking distance was 312 meters, and the time was 304 seconds. The walking trajectory is as Figure 11 shown. The experimenters walked along a rectangular trajectory to ensure that the true trajectory was easy to determine. Figure 12 The comparison of the trajectories of three members obtained by using single-node independent solution and multi-node cooperative positioning algorithms with the true trajectory is shown. It can be seen from the figure that single-node independent solution can effectively limit the speed divergence, but there is a deviation in heading correction, and as time goes by, the deviation accumulates more. After registration optimization, the heading of the members is significantly corrected, the coincidence degree of the trajectory with the true trajectory is greatly improved, and the error is significantly reduced.

[0202] In order to further quantify the positioning effect, the closed positioning error was used as an evaluation index in the experiment. The closed positioning error refers to the ratio of the difference between the calculated position and the initial position when the moving carrier returns to the origin after completing the closed trajectory to the total walking distance. Ideally, the positioning system should return to the initial position, and the closed error is zero. Table 1 shows the closed errors of different positioning methods. The results show that the positioning error during single-node independent solution is about 3.19%, while after multi-node cooperative positioning optimization, the error is reduced to 0.47%, proving that the positioning accuracy of the present invention has been significantly improved.

[0203] Table 1 Closed-loop positioning error of Trajectory 1

[0204]

[0205] Short-distance circular trajectory experiment (Experiment 2): In this experiment, the sensitivity of the algorithm to direction changes was verified. The experimental design was the same as that of Experiment 1, and the reference trajectory for walking was as Figure 13 shown. The total duration of the trajectory completed by the experimenter was 132 seconds, and the walking distance was approximately 107 meters. Figure 14 Shows the experimental results using single-node independent inference and multi-node collaborative positioning algorithms. It can be seen from the figure that the positioning accuracy after registration is significantly improved, and the trajectories of Pedestrian1 and Pedestrian2 are closer to the true trajectory. Although there was a certain deviation in the initial stage of Pedestrian3, it was effectively corrected after registration, and the overall trajectory closure was significantly improved. Table 2 shows the closed-loop positioning error of Experiment 2. The results show that the closed-loop positioning error of single-node inference is 1.21%, while the error drops to 0.57% after multi-node collaborative positioning.

[0206] Table 2 Closed-loop positioning error of Trajectory 2

[0207]

[0208] From the above experimental analysis, it can be seen that the collaborative positioning algorithm based on registration optimization proposed by the present invention can achieve high-precision distributed collaborative positioning without relying on external base stations such as satellites. Whether it is long-distance walking or short-distance walking with rapid direction changes, this algorithm can significantly improve the positioning accuracy. Especially in the case of multi-node collaborative positioning, the error is significantly reduced, which proves the effectiveness and advantages of the algorithm in practical applications.

[0209] In the above embodiments of the present invention, the descriptions of each embodiment have their own emphases. For the parts not detailed in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0210] In several embodiments provided in the present application, it should be understood that the disclosed technical content can be implemented in other ways. Among them, the device embodiments described above are only illustrative. For example, the division of the units can be a logical function division, and there can be other division methods in actual implementation. For example, multiple units or components can be combined or integrated into another device, or some features can be ignored or not executed. Another point is that the couplings or direct couplings or communication connections shown or discussed with each other can be through some interfaces. The indirect couplings or communication connections of the units or modules can be electrical or other forms.

[0211] In addition, in each embodiment of the present invention, each functional unit can be integrated into one processing unit, or each unit can exist physically alone, or two or more units can be integrated into one unit. The above integrated unit can be implemented in the form of hardware or in the form of a software functional unit.

[0212] If the above integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on such an understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in each embodiment of the present invention. The foregoing storage medium includes: various media such as USB flash drives, read-only memories (ROMs), random access memories (RAMs), mobile hard disks, magnetic disks, or optical discs that can store program codes.

[0213] The above are only the preferred embodiments of the present invention. It should be noted that for those of ordinary skill in the art, without departing from the principle of the present invention, several improvements and refinements can be made, and these improvements and refinements should also be regarded as the protection scope of the present invention.

Claims

1. A distributed collaborative positioning method based on registration optimization, characterized in that: The steps include: Acquire the posture data of each node, and solve the posture data using the quaternion posture filtering algorithm; The result of attitude data solution is combined with the speed and acceleration information of inertial navigation, the speed is limited by the zero-speed correction algorithm, and the preliminary position estimate of the node is calculated; Obtain ranging data between nodes and share preliminary location estimates of nodes; According to the distance measurement data and the preliminary position estimation value between nodes, the translation amount and the rotation angle are used as optimization variables, and the optimization objective function with the goal of minimizing the sum of square errors of the distance measurement data is established; The sequential least squares programming algorithm is used to solve the optimization objective function and calculate the corrected translation and rotation angle of each node; The preliminary position estimate of each node is corrected according to the optimization solution results to complete the collaborative positioning between nodes.

2. The distributed collaborative positioning method based on registration optimization according to claim 1, characterized in that: The method of solving the posture data using the quaternion posture filtering algorithm includes: Acquire attitude data collected by multi-node sensors, wherein the attitude data includes three-axis acceleration, three-axis angular velocity, and three-axis geomagnetic intensity; Based on the attitude data, a quaternion attitude filtering algorithm is used to perform 6D attitude estimation and 9D attitude estimation respectively, wherein: 6D attitude estimation is calculated based on the data of three-axis angular velocity and three-axis acceleration; 9D attitude estimation is calculated based on the data of three-axis angular velocity, three-axis acceleration and three-axis geomagnetic intensity; In the posture estimation process, the 6D and 9D estimation steps are decoupled and executed in parallel; The attitude data obtained by decoupling parallel processing is represented by quaternion to complete the attitude calculation of each node.

3. The distributed collaborative positioning method based on registration optimization according to claim 1, characterized in that: The method for obtaining the posture data of each node includes: collecting data from a three-axis gyroscope, a three-axis accelerometer and a three-axis magnetometer through a micro-inertial measurement unit carried on the node, and combining the fixed connection method between the micro-inertial measurement unit and the node foot to obtain original data reflecting the node's motion posture.

4. The distributed collaborative positioning method based on registration optimization according to claim 1, characterized in that: The method of combining the result of attitude data solution with the speed and acceleration information of inertial navigation, limiting the speed through a zero-speed correction algorithm, and calculating the preliminary position estimate of the node includes: Based on the result of attitude solution, the acceleration collected by inertial navigation is projected into the navigation coordinate system, and the gravity acceleration is compensated to obtain the net acceleration in the local geographic coordinate system; Integrate the net acceleration once to calculate the velocity; The improved generalized likelihood ratio method is used to detect the zero speed state, including the following steps: a) performing low-pass filtering on the three-axis acceleration and three-axis angular velocity data within the sampling window to obtain filtered acceleration and angular velocity data; b) Based on the filtered acceleration data, calculate the deviation between the acceleration data and the gravity acceleration in the navigation coordinate system, and take the mean and standard deviation of the deviation as the preliminary features of the stationary state; c) calculating the mean and standard deviation of the filtered angular velocity data as a supplementary feature of the stationary state; d) constructing a generalized likelihood ratio test quantity, combining the preliminary features of the stationary state and the supplementary features of the stationary state into a comprehensive judgment indicator; e) Based on the traditional generalized likelihood ratio detection method, the gait phase continuity detection is introduced to logically associate the comprehensive judgment indicators of multiple consecutive detection windows to determine the zero-speed state; When the zero-speed state is detected, the speed of the period is set to zero, and the acceleration drift is calculated according to the speed change in the zero-speed stage; Compensate the detected acceleration drift, correct the speed, and obtain the corrected speed; The corrected velocity is integrated twice to calculate the preliminary position estimate of the node.

5. The distributed collaborative positioning method based on registration optimization according to claim 1, characterized in that: Methods for obtaining ranging data between nodes and sharing preliminary location estimates of nodes include: Each node collects each other’s ranging data through UWB signals; Each node transmits its own preliminary position estimate calculated by inertial navigation to other nodes via UWB signals, and receives preliminary position estimates from other nodes at the same time; The current distance measurement data is associated with the preliminary position estimate shared between nodes and stored.

6. The distributed collaborative positioning method based on registration optimization according to claim 1, characterized in that: Methods for establishing optimization objective functions include: Based on the distance measurement data between nodes, the distance error between each pair of nodes is calculated; Using the preliminary position estimates of the nodes, determine the relative position difference between each pair of nodes; The distance error and relative position difference between nodes are used as input variables to establish an optimization objective function and determine the optimization variables of the objective function. The objective function takes the sum of squares of deviations between the distance measurement data and the relative position difference between nodes as the minimization target.

7. The distributed collaborative positioning method based on registration optimization according to claim 1, characterized in that: The method of solving the optimization objective function and calculating the corrected translation and rotation angle of each node includes: According to the established optimization objective function, the initial values ​​of the translation amount and the rotation angle are initialized; The optimization objective function is iteratively solved using the sequential least squares programming algorithm; In each iteration, the objective function value and its gradient corresponding to the current translation and rotation angle are calculated; Update the translation and rotation angle according to the gradient information until the objective function value converges or reaches the preset number of iterations; Output the corrected translation and rotation angle obtained by optimization solution.

8. The distributed collaborative positioning method based on registration optimization according to claim 1, characterized in that: The correction method based on optimization solution also includes a mutual registration strategy between nodes, and the mutual registration strategy includes: After each distance measurement, the distance measurement nodes and their corresponding distance measurement data are recorded, and the cumulative distance of the registration segment of each node relative to the current reference node is dynamically updated; When the cumulative distance of the registration segment of the current reference node reaches the preset threshold or meets the optimization condition, the node with the longest cumulative distance is selected as the new reference node; Notify all nodes to update the reference node, clear the accumulated data of their respective registration segments, and redefine the optimization objective function; The distance measurement data in the updated registration segment is optimized and calculated.

9. The distributed collaborative positioning method based on registration optimization according to claim 8, characterized in that: The mutual registration strategy introduces the following constraints in the optimization process: The translation and rotation angle in each registration segment must satisfy the requirement that the rotation angle does not exceed the preset maximum value and the translation does not exceed the set threshold; In the optimization solution process, outliers in the ranging data within the registration segment are excluded; Dynamically update the weight parameters of the optimization objective function.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the fake news detection method based on multimodal information fusion according to any one of claims 1 to 9 are performed.

Images