Robot navigation optimization method based on iterative reweighted least squares

Abstract

This invention relates to the field of robotics, specifically to a robot navigation optimization method based on iterative reweighted least squares. The invention calculates pre-integration results from onboard inertial measurement units to recursively derive the physical trajectory; calculates the geometric difference between cross-node measurement loops and the physical trajectory to generate cross-node trajectory residuals; calculates interaction trust values ​​between nodes using a historical trust sliding window and constructs a node consistency network; generates environmental confidence scores for all nodes based on navigation sensor data; iteratively calculates the globally optimal navigation constraint weight set and generates navigation constraint weights based on the environmental confidence scores; and uses an iterative reweighted least squares algorithm to optimize the spatiotemporal pose graph based on the navigation constraint weights to obtain the navigation trajectory solution. This invention effectively suppresses the influence of false-trust nodes, improves the global consistency and reliability of cluster navigation solutions, and generates a globally geometrically consistent high-precision navigation trajectory solution.

Description

Technical Field

[0001] This invention relates to the field of robotics, specifically to a robot navigation optimization method based on iterative reweighted least squares. Background Technology

[0002] With the development of automation technology, the demand for robot swarms in environments with signal failure is increasing. To address the cumulative errors caused by the inherent drift of a single robot's inertial navigation system, multi-robot cooperative localization and mapping techniques for mutual calibration have become a mainstream research direction. However, existing technologies typically develop along two independent paths when dealing with such extreme environments. When these challenges are coupled, both paths reveal fatal flaws in navigation and surveying applications.

[0003] Some methods employ correlation entropy theory and other techniques to suppress outliers and non-Gaussian noise in acoustic or optical sensor data. While these methods effectively handle interference from the physical environment, their models do not consider that in a dynamic measurement network, other robot nodes may become erroneous measurement benchmarks due to their own malfunctions. Some methods focus on multi-robot collaboration in ideal environments, investigating how to handle geometric constraint conflicts between nodes through voting or outlier removal. However, these methods generally assume that robots can obtain sufficiently reliable features from the environment as anchor points for determining the true location value in the geodetic or local coordinate system. When a robot swarm is deployed in a real underground scenario, the unreliability of both the measurement environment and the measurement benchmark will coexist, forming a more severe, complex technical challenge that existing navigation and surveying technologies cannot solve. This first leads to the problem of measurement benchmark drift and ambiguity of relative constraints: because the self-positioning of all nodes drifts over time, the true error of the relative geometric constraints measured between them becomes difficult to assess, leading to a dilemma where a stable benchmark cannot be established for anomaly detection. Furthermore, it is difficult to distinguish whether an abnormal measurement data originates from a change in the node's own sensor error model or from the node entering a local environment with strong physical field interference, creating a decoupling problem between internal sensor errors and external environmental interference. This could lead to erroneous isolation of normal sensor equipment due to environmental influences. In summary, existing technologies have significant gaps in addressing the dual challenges of unreliable measurement environments and measurement benchmarks. There is an urgent need to develop a cooperative navigation optimization method that can dynamically establish a benchmark for the reliability assessment of relative measurement constraints under such extreme uncertainty through distributed, globally consistent optimization, and effectively distinguish different error sources.

[0004] To address this, a robot navigation optimization method based on iterative reweighted least squares is proposed. Summary of the Invention

[0005] The purpose of this invention is to provide a robot navigation optimization method based on iterative reweighted least squares. The method involves calculating pre-integration results from onboard inertial measurement units to recursively derive the physical trajectory; calculating the geometric difference between cross-node measurement loops and the physical trajectory to generate cross-node trajectory residuals; calculating the interaction trust values ​​between nodes using a historical trust sliding window and constructing a node consistency network; generating environmental confidence scores for all nodes based on navigation sensor data; iteratively calculating the globally optimal navigation constraint weight set and generating navigation constraint weights based on the environmental confidence scores; and using an iterative reweighted least squares algorithm to optimize the spatiotemporal pose graph based on the navigation constraint weights to obtain the navigation trajectory solution. This invention effectively suppresses the influence of false-trust nodes, improves the global consistency and reliability of cluster navigation solutions, and generates a globally geometrically consistent high-precision navigation trajectory solution.

[0006] To achieve the above objectives, the present invention provides the following technical solution: Robot navigation optimization methods based on iterative reweighted least squares include: The local node calculates the pre-integration result of the onboard inertial measurement unit and recursively derives the physical track containing the uncertainty covariance; it receives the uncertainty relative pose information of other nodes as a cross-node measurement loop; it calculates the geometric difference between the cross-node measurement loop and the expected relative pose of the physical track, and uses the uncertainty covariance of each track to normalize the difference and generate the cross-node track residual. An environmental confidence score is generated for all nodes based on the signal quality and feature density of the navigation sensors; the interaction trust value between nodes is calculated by combining the cross-node track residual and historical trust window; a node consistency network is constructed based on all nodes and the interaction trust value; the globally optimal navigation constraint weight set is solved by iterative calculation, and the navigation constraint weights are generated by combining the environmental confidence score. The trajectory propagation factor is constructed from the pre-integration results of the inertial measurement unit, and the weighted cross-node closed-loop factor is constructed by multiplying the cross-node measurement closed loop with the navigation constraint weight. The iterative reweighted least squares algorithm is used to optimize the spatiotemporal pose graph containing the two types of factors to obtain the navigation trajectory solution.

[0007] Preferably, the specific implementation of the self node calculating the pre-integration result of the onboard inertial measurement unit is as follows: the strapdown inertial navigation attitude calculation method based on the three-dimensional rigid body rotating group manifold is used to integrate the original angular velocity measurement value of the inertial measurement unit; the acceleration measurement value of the inertial measurement unit in a time period is transformed to the world coordinate system for integration, and the noise covariance in the integration process is recursively calculated to generate the inertial measurement unit pre-integration factor containing the pose increment, velocity increment and uncertainty covariance, which constitutes the track propagation factor connecting continuous pose nodes in the spatiotemporal pose graph.

[0008] Preferably, receiving relative pose information from other nodes based on external sensor measurements includes at least: using point cloud data from a lidar to calculate the relative pose transformation matrix between the two robot nodes through normal distribution transformation; and using image data from a multi-view stereo camera to calculate the relative pose transformation matrix between the two robot nodes through feature matching and 3D reconstruction.

[0009] Preferably, the specific implementation of calculating the cross-node track residual is as follows: the received cross-node measurement closed loop, i.e., the relative pose information, is defined as an instantaneous measurement constraint; the cross-node track residual is calculated through the following steps: a geometric consistency test loop is constructed in the spatiotemporal pose graph. The loop starts from the current pose of the home node, transforms to the current pose of other nodes through instantaneous measurement constraints, follows the physical track from the historical pose to the current pose of other nodes, performs cross-node transformation along the historically established cooperative observation factors connecting other nodes and the home node, and follows the physical track from the historical pose to the current pose of the home node; the difference between the cumulative pose transformation matrix and the identity matrix of the test loop is calculated as the cross-node track residual characterizing geometric inconsistency.

[0010] Preferably, the function used to calculate the interaction trust value is a graph-based double-weighted function; when the magnitude of the cross-node track residual exceeds a preset threshold, the interaction trust value calculated by the graph-based double-weighted function will rapidly drop to zero, suppressing moderate noise impact and malicious or faulty data with large deviations generated by false trust nodes. The historical trust sliding window is a data queue maintained for each other node to store its consistency performance records of all interactions within a recent finite period, so as to incorporate its historical reputation when calculating the current trust value. When processing the historical interaction consistency performance of other nodes stored in the historical trust sliding window, an exponentially weighted moving average algorithm is specifically used. When calculating the interaction trust value, the residual performance of each interaction in history within the sliding window is given a weight that decays exponentially over time, so that the trust assessment model can quickly respond to the latest changes in the state of other nodes.

[0011] Preferably, the node consensus network is a weighted directed graph, the vertex set of which consists of all robot nodes in the cluster, and the directed edges from the id node to the other node, with the weight of the edge set to the calculated interaction trust value. The specific implementation of solving the globally optimal navigation constraint weight set through iterative calculation is as follows: each navigation constraint weight in the node consensus network is iteratively updated. In the (k+1)th iteration, the navigation constraint weight from the id node to the other node is determined by a propagation function. The input of the function includes at least: the navigation constraint weight between the id node and other nodes in the kth iteration; and a network consensus support term that is a weighted aggregation of the navigation constraint weights of all other third-party nodes in the cluster to the id node, where the weight used for aggregation depends on the navigation constraint weight of the id node to the third-party nodes. The iterative process continues until the change in all navigation constraint weights in the graph is less than the convergence threshold, reaching an equilibrium state, thus completing the navigation constraint weights of swarm intelligence rather than the partial observation of a single node.

[0012] Preferably, the specific implementation of the iterative reweighted least squares algorithm for optimizing the spatiotemporal pose graph containing two types of factors is as follows: the Levenburg-Marquardt algorithm is used as the nonlinear least squares solver. In each iteration, the Levenburg-Marquardt algorithm dynamically switches between the Gauss-Newton method and the gradient descent method by introducing a damping factor, so that even if the initial pose estimation is poor and the optimization problem is highly nonlinear, the entire spatiotemporal pose graph solution process still converges stably.

[0013] Preferably, the spatiotemporal pose graph is optimized using an iterative reweighted least squares algorithm. The specific implementation process includes: synchronously updating the navigation state of each node in the cluster in each optimization iteration, and recalculating and updating the navigation constraint weights of other nodes based on the updated navigation state, until the navigation state converges, and obtaining a navigation trajectory solution that is physically feasible, globally geometrically consistent, and has suppressed the influence of false belief nodes. The iterative process is implemented through a nested double loop structure: In the outer loop, the closed-loop residuals of all cross-node tracks are calculated based on the current navigation state, and the navigation constraint weights of all other nodes are updated to form a fixed weight set in this outer loop; In the inner loop, the fixed weight set passed from the outer loop is used to modulate the weighted cross-node closed-loop factors, and the Levenburg-Marquardt algorithm is run for multiple iterations until the spatiotemporal pose map converges under the current weights, obtaining the updated navigation state; The updated navigation state output by the inner loop is used to return to the starting point of the outer loop to begin the next residual calculation and weight update.

[0014] Compared with the prior art, the beneficial effects of the present invention are as follows: 1. This invention introduces uncertainty-normalized geometric residuals. When verifying the consistency of information between nodes, it no longer simply compares measured values, but takes into account the covariance of each node's own positioning uncertainty. This solves the problem of blind verification caused by the lack of reliable benchmarks in environments with satellite navigation failure and sparse features. It can accurately identify geometric model conflicts introduced by other faulty or malicious nodes even when its own positioning accuracy is not high. This ability to cope with the dual unreliability of the external environment and internal members improves the robustness of the entire robot swarm navigation, enabling it to maintain stable collaborative operation capabilities in extremely harsh environments.

[0015] 2. This invention solves the ambiguity problem of fault attribution by introducing environmental observation confidence as a modulation factor in navigation constraint weights. It distinguishes whether the geometric residual originates from a truly faulty false-signal node or a healthy node temporarily entering a harsh environment such as geomagnetic disturbance or feature sparsity. Existing technologies often incorrectly isolate the latter, impairing the overall capability of the cluster. This invention, by giving higher tolerance to nodes in low-confidence environments, avoids mistakenly eliminating healthy nodes, thereby maximizing the preservation of the cluster network integrity, ensuring the continuous participation of all available nodes, and significantly improving the coverage and completion efficiency of collaborative mapping or search and rescue missions.

[0016] 3. This invention ensures that the weights applied to the final optimization are globally self-consistent by constructing a geometrically consistent network between nodes and iteratively solving for the globally optimal navigation constraint weight set. This mechanism fundamentally eliminates the risk of local, erroneous geometric constraints tearing or distorting the entire spatiotemporal pose graph. Therefore, the navigation trajectory solution obtained by this invention is not only locally optimal but also globally geometrically consistent. In practical applications, this means that this invention can ensure that the entire robot swarm produces a single, seamlessly stitched, high-precision 3D map or collaborative trajectory, meeting the high standards required for precision measurement and surveying. Attached Figure Description

[0017] Figure 1 A flowchart of a robot navigation optimization method based on iterative reweighted least squares provided in an embodiment of the present invention; Figure 2 A schematic diagram of the geometric consistency network and weight optimization between nodes provided in an embodiment of the present invention; Figure 3 The flowchart illustrates the specific implementation of iterative reweighted least squares provided in this embodiment of the invention. Detailed Implementation

[0018] 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 of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0019] Please see Figures 1 to 3 This invention provides a robot navigation optimization method based on iterative reweighted least squares, the technical solution of which is as follows: Robot navigation optimization methods based on iterative reweighted least squares include: The local node calculates the pre-integration result of the onboard inertial measurement unit and recursively derives the physical track containing the uncertainty covariance; it receives the uncertainty relative pose information of other nodes as a cross-node measurement loop; it calculates the geometric difference between the cross-node measurement loop and the expected relative pose of the physical track, and uses the uncertainty covariance of each track to normalize the difference and generate the cross-node track residual. Environmental confidence scores are generated for all nodes based on the signal quality and feature density of navigation sensors. Specifically, a normal upper and lower limit range is set for key performance indicators of navigation sensors, such as the point cloud density of LiDAR or the number of effective feature points of the camera. A normalized score is determined based on the position of the real-time measurement value within this range. Finally, the scores of different sensors are weighted and fused to obtain a unified environmental confidence score value. The interaction trust value between nodes is calculated by combining the cross-node track residual and historical trust window. A node consensus network is constructed based on all nodes and the interaction trust value. The globally optimal navigation constraint weight set is solved through iterative calculation, and navigation constraint weights are generated by combining the environmental confidence score. Specifically, the interaction trust value after network consensus iteration is directly multiplied by the environmental confidence value of the location of other nodes. Through this adjustment, the information weight provided by a reliable node is adaptively reduced when entering a harsh environment.

[0020] The trajectory propagation factor is constructed from the pre-integration results of the inertial measurement unit, and the weighted cross-node closed-loop factor is constructed by multiplying the cross-node measurement closed loop with the navigation constraint weight. The iterative reweighted least squares algorithm is used to optimize the spatiotemporal pose graph containing the two types of factors to obtain the navigation trajectory solution.

[0021] Example 1: One application scenario of this invention involves a cluster of 10 identical robots deployed in an abandoned mine to perform collaborative exploration and 3D modeling tasks. The mine environment has the following typical characteristics: complete GNSS signal interruption; highly repetitive tunnel structures lacking unique visual or geometric features; magnetic field anomalies in some areas; and dust that may affect the performance of optical sensors.

[0022] As one embodiment of the present invention, refer to Figure 1 The flowchart of the robot navigation optimization method based on iterative reweighted least squares is shown below. Figure 2 A schematic diagram of the geometric consistency network between nodes and weight optimization, refer to... Figure 3 The flowchart shows the specific implementation method of iterative reweighted least squares.

[0023] Furthermore, the specific implementation method of the self node in calculating the pre-integration result of the onboard inertial measurement unit is as follows: the strapdown inertial navigation attitude calculation method based on the three-dimensional rigid body rotating group manifold is used to integrate the original angular velocity measurement value of the inertial measurement unit; the acceleration measurement value of the inertial measurement unit in a time period is transformed to the world coordinate system for integration, and the noise covariance in the integration process is recursively calculated to generate the inertial measurement unit pre-integration factor containing the pose increment, velocity increment and uncertainty covariance, which constitutes the track propagation factor connecting continuous pose nodes in the spatiotemporal pose graph.

[0024] Specifically, in At a given moment, robot A (the id node) in the cluster is moving in a straight line along the mine roadway, calculating its current pose and velocity. In the subsequent... arrive Within this 100-millisecond time window, robot A begins to turn right. During this period, its inertial measurement unit continuously outputs raw measurement data at a frequency of 1000 Hz, collecting 100 sets of angular velocity and acceleration measurements within these 100 ms. For example, the angular velocity measurements in the first 50 ms are close to zero on the X, Y, and Z axes, while in the last 50 ms, the angular velocity on the Z-axis (yaw axis) is stably output at approximately 5.7 degrees / second. Simultaneously, the acceleration measurement on the X-axis (forward direction) remains consistently at approximately 0.5 m / s². 2 The Z-axis is affected by gravitational acceleration. A strapdown inertial navigation attitude calculation method based on a three-dimensional rigid body rotation manifold is employed to integrate these 100 sets of angular velocity measurements. The calculation is performed directly on the rotation matrix manifold, fundamentally avoiding the gimbal lock-up problem that may occur when using Euler angles or quaternions, ensuring the continuity and accuracy of the attitude calculation. After accurately calculating the attitude change for each millisecond within these 100ms, the corresponding acceleration measurements are precisely converted from the robot's body coordinate system to... Integration is then performed in a fixed world coordinate system to calculate the velocity and position increments. Simultaneously, based on a pre-calibrated noise model of the inertial measurement unit (IMU) sensor, the noise covariance during the integration process is recursively calculated. Finally, in At time 100ms, an inertial measurement unit pre-integration factor is generated, which encapsulates the complete relative motion information within 100ms: for example, the pose increment is { =0.05m, =0.001m, =0.285°}, where This represents the robot's position change along the X-axis. This represents the robot's position change along the Y-axis. The attitude change representing the robot's yaw angle, with the velocity increment being... =0.05m / s, and a 9x9 covariance matrix that quantifies the degree of uncertainty of all the above incremental factors; this factor is then constructed as a connection and The trajectory propagation factors of the two pose nodes provide high-precision, drift-free local motion constraints for the entire spatiotemporal pose graph.

[0025] The specific implementation method of pre-integration of the inertial measurement unit is defined. A strapdown inertial navigation attitude calculation method based on a three-dimensional rigid body rotating manifold is adopted, which avoids the singularity problem in traditional methods and accurately recursively calculates the error covariance, providing a high-precision, drift-free basis for relative motion constraints, and significantly improving the accuracy and stability of physical trajectory estimation.

[0026] Furthermore, receiving relative pose information from other nodes based on external sensor measurements includes at least: a relative pose transformation matrix between two robot nodes calculated using point cloud data from a lidar system through normal distribution transformation; and a relative pose transformation matrix between two robot nodes calculated using image data from a multi-view stereo camera through feature matching and 3D reconstruction.

[0027] Specifically, robot A and robot B travel towards each other in the same mine tunnel, approximately 5 meters apart. Robot A's LiDAR continuously generates a 3D point cloud of the surrounding environment; robot B, via UWB communication, broadcasts a local point cloud that constitutes its own shape to A; robot A's processor segments point cloud clusters belonging to B from its own real-time point cloud scan; a normal distribution transformation algorithm is used to fit both point clouds into a set of Gaussian distributions, and through 15 iterations of optimization, a relative pose transformation matrix is ​​found that maximizes the probability overlap between the two Gaussian distributions. Finally, the pose of A relative to B is calculated as { =5.12m, =-1.03m, =15.2°}, and based on the convergence score of the NDT algorithm, a 6x6 covariance matrix representing the measurement uncertainty is generated.

[0028] Simultaneously, robot A's stereo camera also captured robot B. Using the ORB feature extraction algorithm, 35 feature points were successfully matched in the left and right images within B's predefined chassis area. Utilizing camera intrinsic and extrinsic parameters, triangulation was used to reconstruct the 3D coordinates of these 35 feature points in camera A's coordinate system. These 3D points were then aligned with the known 3D model of robot B's chassis, and the relative pose of A with respect to B was calculated using a PnP solver. The results of the two parallel calculations were fused to generate a high-precision, cross-node measurement closed loop with reliable uncertainty assessment, serving as key input information for subsequent navigation optimization.

[0029] The specific sensors and methods for acquiring external relative pose information were defined. By explicitly using two complementary sensors, LiDAR and a stereo camera, adaptability to different environments (such as smoke and low light) was enhanced, ensuring a reliable source for cross-node measurement loops.

[0030] Furthermore, the specific implementation method for calculating the cross-node track residual is as follows: the received cross-node measurement closed loop, i.e., the relative pose information, is defined as an instantaneous measurement constraint; the cross-node track residual is calculated through the following steps: a geometric consistency check loop is constructed in the spatiotemporal pose graph. The loop starts from the current pose of the local node and transforms to the current pose of the other node through the instantaneous measurement constraint. The physical path from the historical pose to the current pose along the nodes of others. Along the historically established collaborative observation factors connecting other nodes and the self node Perform cross-node transformations, following the physical trajectory from the historical pose to the current pose along the current node. The difference between the cumulative pose transformation matrix and the identity matrix of the test loop is calculated as the cross-node track residual characterizing geometric inconsistency. The specific formula is: ; in: This represents the cross-node track residual, which is typically a 6-dimensional vector in robot navigation; Represents the pose transformation matrix; Representing a logarithmic mapping, The error of the transformation matrix is ​​mapped to its corresponding The Lie algebra vector has a magnitude (norm) that is the residual magnitude. This indicates the inverse transformation; when the loop is not closed, the residual vector is not zero.

[0031] Specifically, robot A, in order to verify the time from robot B at time... The reliability of a newly received relative pose measurement, i.e., an instantaneous measurement constraint. Known information: The new measurement data is from... At a given moment, A measures B to be 3.0 meters in front of it; the historical constraint is in the past. At that moment, there is a highly reliable record that B was 2.0 meters ahead of A; A's trajectory was from... arrive A's own inertial measurement unit pre-integration showed that it had advanced 1.0 meter; B's trajectory was from... arrive B's own inertial measurement unit pre-integration shows that it remains stationary.

[0032] Construct a virtual closed-loop path to verify consistency, assuming A is in... The starting point is the origin (0, 0): from the starting point (0, 0) of A, the coordinates are transformed to point B through a new measurement. The current position of B is (3.0, 0); B starts from... arrive Remaining still, therefore it is in The position of B, relative to the starting point of A, is still (3.0, 0); The position of A is calculated by reverse transformation using historical constraints (B is 2.0 meters in front of A). The position of A should be (3.0-2.0, 0) = (1.0, 0); From the calculated position of A... The position (1.0, 0) changes along A's own track (moving forward 1.0 meters), and the final position back to the starting point should be (1.0 + 1.0, 0) = (2.0, 0).

[0033] Ideally, the endpoint after the loop closure should completely coincide with the starting point (0, 0), but the calculated result is (2.0, 0), that is, the difference between the cumulative pose transformation matrix and the identity matrix is ​​2.0 meters. After uncertainty covariance normalization, it is used as the cross-node track residual characterizing the geometric inconsistency of this new measurement, and is used for subsequent trust assessment.

[0034] The specific method for calculating geometric residuals is defined. By constructing a rigorous "geometric consistency check loop," the fuzzy "comparison" process is concretized, giving the calculation of residuals irrefutable physical meaning and mathematical rigor, thus providing a solid and reliable input for subsequent trust assessment.

[0035] Furthermore, the function used to calculate the interaction trust value is specifically a graph-based double-weighted function; when the magnitude of the cross-node track residual exceeds a preset threshold, the interaction trust value calculated by the graph-based double-weighted function will rapidly drop to zero, suppressing moderate noise impact and malicious or faulty data with extremely large deviations generated by false trust nodes; its mathematical model can be expressed as: ,in, Indicates the instantaneous trust value; This represents the magnitude of the cross-node track residuals after uncertainty covariance normalization; This represents a preset threshold. This threshold is determined based on statistical principles and is used to distinguish between normal fluctuations and true outliers. When the residual modulus exceeds... At that time, the trust value quickly dropped to 0.

[0036] When processing the consistency performance of historical interactions between other nodes stored within the historical trust sliding window, an exponentially weighted moving average algorithm is specifically used. When calculating the interaction trust value, the residual performance of each historical interaction within the sliding window is assigned a weight that decays exponentially over time, enabling the trust assessment model to quickly respond to the latest changes in the states of other nodes. Its mathematical model can be expressed as: ;in, express The final interaction trust value updated in real time; This represents the current instantaneous confidence value calculated by the graph basis double-weighting function; This represents the historical trust scores stored in the historical trust slide window; The smoothing factor determines how quickly the trust assessment model responds to the latest changes.

[0037] Specifically, robot A is continuously evaluating the reliability of its cooperative navigation information with neighboring robot B. A hard threshold for the modulus of the uncertainty-normalized geometric residual is preset at c=4.685. At a certain moment, B's sensor suddenly malfunctions, causing a serious error in its broadcast pose information, and the calculated residual spikes to 10.8. Since this far exceeds the threshold of 4.685, the descent property of the graph basis double weighting function is activated, directly outputting an instantaneous trust value of 0.0, deterministically filtering out this extreme anomaly before it enters the historical evaluation. Combined with the new instantaneous trust value of 0.0, B's historical trust score is updated to 0.3*0.0 + (1-0.3)*0.98 ≈ 0.69. Although B has a good historical record, this one extreme anomaly significantly and rapidly lowers its reputation score. The final output historical trust score of 0.69 is A's final interactive trust value for B at the current moment. It reflects both the decisive suppression of extreme outliers and the dynamic memory of its historical behavior, achieving robust and sensitive trust evaluation.

[0038] The specific function and algorithm for calculating the initial interaction trust value are defined. The "re-descent" property of the graph basis dual-weight function can decisively and deterministically filter out extreme outliers; while the exponentially weighted moving average algorithm improves the response speed of the trust assessment model to changes in node state. The combination of the two constructs an initial trust assessment module that is both robust and sensitive.

[0039] Furthermore, the specific implementation of solving the globally optimal navigation constraint weight set through iterative calculation is as follows: Each navigation constraint weight in the node consensus network is iteratively updated. In the (k+1)th iteration, the navigation constraint weight from the id node to other id nodes is determined by a propagation function. The propagation function weighted and fused the id node's direct trust in other id nodes with the indirect evaluations of other id nodes in the network. Indirect evaluations will focus more on the opinions of third-party nodes that the id node trusts more, ultimately outputting a new weight that balances direct observation and network consensus. The function's input includes at least: the navigation constraint weights between the id node and other id nodes in the kth iteration; and a network consensus support term that is a weighted aggregation of the navigation constraint weights of all other third-party nodes in the cluster, where the aggregation weights depend on the id node's navigation constraint weights to third-party nodes. The iterative process continues until the change in all navigation constraint weights in the graph is less than the convergence threshold, reaching an equilibrium state, thus completing the navigation constraint weights of swarm intelligence rather than the one-sided observations of a single node.

[0040] Specifically, in At a certain point, robot A in the cluster needs to determine its final navigation constraint weights on robot B. At this time, robots C and D (third-party nodes) also exist in the network; the initial consensus metric among the nodes has been calculated: A's direct trust in B. The value is 0.5; A's direct trust in C. The value is 0.9; A's direct trust in D. The value is 0.2; and C's trust in B is... The value is 0.95, while D's trust in B is... Only 0.1.

[0041] Start iteratively updating the weights of A with respect to B. Its new weight depends not only on the initial 0.5, but more importantly on a network consistency support term; the support term is calculated as follows: Evaluating the recommendation from C: A's trust in C is 0.9, and C's trust in B is 0.95, representing a strong positive recommendation from a trusted third party; Evaluating the recommendation from D: A's trust in D is only 0.2, and D's trust in B is 0.1, representing a weak negative recommendation from an untrusted third party; After weighted aggregation, the strong positive recommendation from C dominates, so even though A's direct observation trust in B is only 0.5, the network consistency support term will significantly increase it; After one iteration, The weights are updated from 0.5 to 0.85, and this process continues across all nodes in the network until all weights converge and stabilize. Ultimately, the navigation constraint weights of A on B reflect the collective consensus of the entire cluster on B, rather than a single, potentially erroneous observation by A.

[0042] The mean and trend of navigation constraint weights for each node are tracked. When an irreversible, slow decline trend occurs, the node is marked as sub-healthy. An online collaborative calibration procedure is automatically triggered to coordinate the sub-healthy node and a healthy node with the highest current navigation constraint weight to jointly execute a preset calibration path. The collaborative observation data during this process is used to re-estimate and correct the zero bias of the inertial measurement unit of the sub-healthy node online.

[0043] By introducing a slow variable monitoring and proactive collaborative calibration mechanism for health status, the cluster is equipped with the ability to self-diagnose and self-repair. It can proactively detect and compensate for model mismatch caused by physical factors such as sensor zero-bias drift and wheel wear, ensuring that the entire cluster can maintain a high standard of navigation accuracy throughout its entire life cycle, and greatly improving the reliability, stability and data consistency in long-term autonomous operation and maintenance tasks.

[0044] The specific iterative method for solving the globally optimal weight set is limited. By introducing a "network consensus support term," trust assessment is elevated from an isolated judgment between "two points" to a collective decision based on the "whole network topology," enabling the final weights to reflect the collective consensus of the cluster and greatly enhancing the resistance to coordinated deception or large-scale associated failures.

[0045] Furthermore, the specific implementation of the iterative reweighted least squares algorithm for optimizing the spatiotemporal pose graph containing two types of factors is as follows: the Levenburg-Marquardt algorithm is used as the nonlinear least squares solver. In each iteration, the Levenburg-Marquardt algorithm dynamically switches between the Gauss-Newton method and the gradient descent method by introducing a damping factor, so that even if the initial pose estimation is poor and the optimization problem is highly nonlinear, the entire spatiotemporal pose graph solution process still converges stably.

[0046] Specifically, in one inner loop of the iterative reweighted least squares algorithm, the spatiotemporal pose graph containing 10 robot nodes and a total of 500 pose nodes needs to be optimized. Due to the long-term cumulative drift of the inertial measurement unit, the initial consistency of the spatiotemporal pose graph is poor, and the initial total error of the optimization problem is as high as 1500. In the first few iterations of optimization, the problem exhibits high nonlinearity because the current pose estimate deviates significantly from the true value. At this time, the Levenburg-Marquardt algorithm automatically sets a large damping factor, making the behavior of the solution step approximate the gradient descent method. Although the step size of this step is small, the direction is stable, which can ensure that the total error decreases steadily. For example, after the first iteration, the total error steadily decreases from 1500 to 1200, avoiding the risk of optimization divergence. After about 10 iterations, most of the poses in the graph have been adjusted to positions close to the optimal solution, and the total error has been reduced to 50. The problem exhibits good quadratic characteristics in the local region. The Levenburg-Marquardt algorithm automatically reduces the damping factor, transforming the behavior of the solution steps into that of the Gauss-Newton method. It utilizes second-order gradient information to approach the optimal solution with a larger step size and a faster speed. For example, after the eleventh iteration, the total error may be reduced sharply from 50 to 15.

[0047] The spatiotemporal pose graph also includes energy state nodes, which represent and update the remaining power of each robot node; by analyzing the boundaries and uncertainties of the 3D map constructed in the spatiotemporal pose graph in real time, the information gain of each unknown area is calculated; and by combining the real-time energy state of each node, the estimated energy consumption to reach the target area and the energy requirements of the return path, multi-objective optimization calculations are performed to generate the optimal navigation target point.

[0048] By quantifying and coordinating the optimization of energy management and exploration desire, the invention can intelligently achieve the best balance between maximizing exploration efficiency and ensuring the safe return of all nodes, avoiding robot loss and mission failure due to energy depletion that leads to no return. This greatly enhances the autonomy, safety, and practical value of the invention in real, long-endurance missions.

[0049] The specific solver for backend optimization was limited. The Levenburg-Marquardt algorithm was adopted to ensure stable convergence of the optimization solution process when the initial error of pose estimation is large or the problem is highly nonlinear, thus avoiding algorithm divergence and improving the success rate and reliability of navigation solutions.

[0050] Furthermore, the spatiotemporal pose graph is optimized using an iterative reweighted least squares algorithm. The specific implementation process includes: synchronously updating the navigation state of each node in the cluster in each optimization iteration, and recalculating and updating the navigation constraint weights of other nodes based on the updated navigation state, until the navigation state converges, and obtaining a physically feasible and globally geometrically consistent navigation trajectory solution that has suppressed the influence of false belief nodes. The iterative process is implemented through a nested double loop structure: In the outer loop, the closed-loop residuals of all cross-node tracks are calculated based on the current navigation state, and the navigation constraint weights of all other nodes are updated to form a fixed weight set in this outer loop; In the inner loop, the fixed weight set passed from the outer loop is used to modulate the weighted cross-node closed-loop factors, and the Levenburg-Marquardt algorithm is run for multiple iterations until the spatiotemporal pose map converges under the current weights, obtaining the updated navigation state; The updated navigation state output by the inner loop is used to return to the starting point of the outer loop to begin the next residual calculation and weight update.

[0051] Specifically, in At a certain moment, a global optimization was triggered. Due to severe drift in the inertial measurement units of some robots (such as robot C), the initial total error of the entire spatiotemporal pose map reached as high as 2500. First, the outer loop was entered, and the closed-loop residuals of all cross-node paths were calculated based on the relatively coarse navigation states of each node. Based on these residuals, the navigation constraint weights of all cross-node constraints were updated. For example, the constraint between healthy nodes A and B received a high weight due to its small residual. =0.95; while the constraint between the faulty node C and A is assigned a very low weight due to the huge residual. =0.05; In this outer loop, { =0.95, =0.05} This set of weights is fixed.

[0052] Next, the inner loop modulates all weighted cross-node closure factors using the fixed weight set passed from the outer loop. The error constraint provided by node C becomes insignificant due to its low weight of 0.05. The Levenburg-Marquardt algorithm in the inner loop begins iteratively optimizing the navigation state pose of all nodes. After 15 iterations of the Levenburg-Marquardt algorithm, the inner loop converges, and the poses of all nodes are updated. Due to the primary reliance on high-weight health constraints, the total error of the entire spatiotemporal pose graph significantly decreases to 400. The optimized new navigation state output from the inner loop is returned to the starting point of the outer loop. Based on this more accurate navigation state, the second iteration of the outer loop begins, recalculating the residuals and generating a more accurate weight set. This nested, alternating optimization process continues until both the navigation state and the weight set converge and stabilize, ultimately obtaining a globally consistent navigation trajectory solution that has completely suppressed the influence of false belief nodes.

[0053] An adaptive optimization strategy scheduler is constructed to monitor the global health status of the geometric consistency network between nodes in real time. The health status is characterized by the mean and variance of all navigation constraint weights. The execution strategy of the iterative reweighted least squares algorithm is dynamically adjusted according to the health status: when the mean is higher than the mean threshold and the variance is lower than the variance threshold, the scheduler will reduce the execution frequency of the outer loop in the nested double loop structure and prioritize the allocation of computing resources to the state optimization of the inner loop; when the mean of the weights is lower than the mean threshold or the variance is higher than the variance threshold, the scheduler will increase the number of iterations of the outer loop and increase the initial damping factor of the Levenburg-Marquardt algorithm in the inner loop.

[0054] The optimizer of this invention is upgraded from a fixed-parameter execution unit to a thinking, self-regulating intelligent solver capable of sensing the difficulty of the current problem in real time: when the cluster state is stable and consensus is high, computing power is used to improve accuracy and frequency; while when the cluster state is chaotic and conflicts are severe, computing power is used to ensure convergence stability. This intelligent and adaptive allocation of computing resources ensures that this invention can achieve the best balance between computational efficiency and robustness under any operating conditions.

[0055] This defines the core iterative mechanism of the entire iterative reweighted least squares algorithm. By using an inner and outer double loop structure, the two coupled problems of weight estimation and state optimization are decoupled, ensuring the stability and efficient convergence of the entire adaptive control process, enabling the rapid finding of the optimal navigation solution in dynamically changing environments and trust relationships.

[0056] By combining uncertainty quantification, historical trust, environmental awareness, and global weight optimization, the stability problem of collaborative navigation in a "double unreliable" environment is fundamentally solved.

[0057] Example 2: As one embodiment of the present invention, refer to Figure 1 The flowchart of the robot navigation optimization method based on iterative reweighted least squares is shown below. Figure 2 A schematic diagram of the geometric consistency network between nodes and weight optimization, refer to... Figure 3 The flowchart shows the specific implementation method of iterative reweighted least squares.

[0058] This embodiment continues the collaborative exploration scenario in the mine described in Embodiment 1, aiming to explain in detail how the iterative reweighted least squares algorithm operates as a dynamic and adaptive optimization control process.

[0059] exist At a certain moment, a global optimization is triggered. Due to severe drift in the inertial measurement units of some robots (such as robot C), they broadcast incorrect relative pose information, resulting in poor initial consistency of the entire spatiotemporal pose graph. The calculated initial total error is as high as 2500.0. At this point, the adaptive optimization control flow of the iterative reweighted least squares algorithm is initiated, as follows: First outer loop iteration: The first step is the weight calculation phase of the outer loop. Based on the relatively coarse navigation status of each node, the cross-node track residuals of all cross-node constraints are calculated. Environmental confidence scores are generated for all nodes. The interaction trust values ​​between nodes are calculated using the cross-node track residuals and historical trust windows. A node consistency network is constructed based on all nodes and the interaction trust values. The globally optimal navigation constraint weight set is solved through iterative calculation, and navigation constraint weights are generated using the environmental confidence scores. For the constraint with high geometric consistency between healthy nodes A and B, its residual magnitude is only 0.8, much smaller than the threshold of 4.685 for the graph basis double weight function, and its historical interaction performance is good. Finally, after iterative optimization of the network consistency support term, a high navigation constraint weight close to 1 is assigned to it, for example... =0.95. For the constraints between the faulty node C and the healthy node A, the residual magnitude is as high as 12.5, far exceeding the threshold. The graph basis double weighting function directly outputs a zero-confidence value, resulting in the final navigation constraint weights being assigned an extremely low penalty value, for example... =0.05. In this outer loop, { =0.95, =0.05} This complete set of weights was calculated and temporarily fixed.

[0060] Next, the inner loop enters the state optimization phase, using the fixed weight set passed from the outer loop to modulate all weighted cross-node closure factors in the spatiotemporal pose graph. At this point, the constraint provided by the faulty node C, with its huge geometric error, has its influence on the entire optimization problem minimized to almost negligible due to its extremely low weight of 0.05. Subsequently, the Levenburg-Marquardt algorithm in the inner loop begins nonlinear iterative optimization of the navigation poses of all nodes. Since it mainly relies on reliable constraints with weights close to 1 from healthy nodes, the Levenburg-Marquardt algorithm converges quickly and stably. After 15 iterations of the Levenburg-Marquardt algorithm, the inner loop converges, and the poses of all nodes are significantly updated; the total error of the entire spatiotemporal pose graph decreases dramatically from the initial 2500.0 to 400.0 due to the successful suppression of the erroneous constraints.

[0061] Second outer loop iteration: The optimized new navigation state output from the inner loop is returned to the starting point of the outer loop to begin the second iteration. Based on this more accurate navigation state, the cross-node track residuals for all nodes are recalculated. Since the poses of healthy nodes A and B are more accurate, their residuals become smaller, potentially increasing their navigation constraint weights from 0.95 to 0.98. Meanwhile, the residuals of the faulty node C remain large, and its weights may be further reduced to 0.01. Using this more refined weight set, the loop re-enters for state optimization. Due to the more accurate weight allocation, this optimization will proceed from a better starting point, potentially reducing the total error from 400.0 to 350.0.

[0062] This alternating optimization process, consisting of nested inner and outer loops—weight calculation → state optimization → weight recalculation → state reoptimization—continues continuously. After 3 to 5 iterations of the outer loop, the total error change will be less than a preset threshold, and both the navigation state and the weight set will reach stable convergence. At this point, the navigation trajectory solution is a physically feasible and globally geometrically consistent final result that has completely suppressed the influence of false signal nodes.

[0063] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A robot navigation optimization method based on iterative reweighted least squares, characterized in that, include: The id node calculates the pre-integration results from the onboard inertial measurement unit and recursively derives the physical track containing the uncertainty covariance. The ego node is the robot currently performing calculations and decisions; it receives uncertain relative pose information from other ego nodes as a cross-node measurement loop; the other ego node is the robot currently being directly observed and evaluated by the ego node. Calculate the geometric difference between the cross-node measurement closed loop and the expected relative pose of the physical track, and normalize the difference using the uncertainty covariance of each track to generate the cross-node track residual; An environmental confidence score is generated for all nodes based on the signal quality and feature density of the navigation sensors; the interaction trust value between nodes is calculated by combining the cross-node track residual and historical trust window; a node consistency network is constructed based on all nodes and the interaction trust value; the globally optimal navigation constraint weight set is solved by iterative calculation, and the navigation constraint weight is generated by multiplying it with the environmental confidence score. The trajectory propagation factor is constructed from the pre-integration results of the inertial measurement unit, and the weighted cross-node closed-loop factor is constructed by multiplying the cross-node measurement closed loop with the navigation constraint weight. The iterative reweighted least squares algorithm is used to optimize the spatiotemporal pose graph containing the two types of factors to obtain the navigation trajectory solution.

2. The robot navigation optimization method based on iterative reweighted least squares according to claim 1, characterized in that, The specific implementation of the self node in calculating the pre-integration result of the onboard inertial measurement unit is as follows: the strapdown inertial navigation attitude calculation method based on the three-dimensional rigid body rotating manifold is used to integrate the original angular velocity measurement value of the inertial measurement unit; the acceleration measurement value of the inertial measurement unit in a time period is transformed to the world coordinate system for integration, and the noise covariance in the integration process is recursively calculated to generate the inertial measurement unit pre-integration factor containing the pose increment, velocity increment and uncertainty covariance, which constitutes the track propagation factor connecting continuous pose nodes in the spatiotemporal pose graph; The process of calculating the pre-integration result by the inertial measurement unit is to integrate the angular velocity measurement value using a solution method based on the three-dimensional rigid body rotating manifold to obtain the attitude increment, and then use this attitude to transform the acceleration measurement value to the world coordinate system and integrate it again to obtain the velocity and position increment. At the same time, the sensor noise covariance is recursively calculated throughout this integration process, and finally the pre-integration result is generated.

3. The robot navigation optimization method based on iterative reweighted least squares according to claim 1, characterized in that, Receiving relative pose information from other nodes based on external sensor measurements includes at least: a relative pose transformation matrix between two robot nodes calculated using point cloud data from a lidar system through normal distribution transformation; and a relative pose transformation matrix between two robot nodes calculated using image data from a multi-view stereo camera through feature matching and 3D reconstruction.

4. The robot navigation optimization method based on iterative reweighted least squares according to claim 1, characterized in that, The specific implementation method for calculating cross-node track residuals is as follows: the received cross-node measurement closed loop, i.e., relative pose information, is defined as instantaneous measurement constraint; The cross-node track residuals are calculated using the following steps: A geometric consistency check loop is constructed in the spatiotemporal pose graph. The loop starts from the current pose of the home node, transforms to the current pose of the other node through instantaneous measurement constraints, follows the physical track from the historical pose to the current pose of the other node, performs cross-node transformation along the historically established cooperative observation factors connecting the home and other nodes, and follows the physical track from the historical pose to the current pose of the home node. The difference between the cumulative pose transformation matrix and the identity matrix of the check loop is calculated as the cross-node track residuals characterizing geometric inconsistency.

5. The robot navigation optimization method based on iterative reweighted least squares according to claim 1, characterized in that, An exponentially weighted moving average algorithm is used to calculate the interactive trust value by weighting and fusing the instantaneous trust performance calculated based on the current cross-node track residuals using a graph-based dual-weighting function with the historical consistency performance recorded in the historical trust sliding window. The graph-based dual-weighting function is configured such that when the magnitude of the cross-node track residuals exceeds a preset threshold, the interactive trust value calculated by the graph-based dual-weighting function will rapidly drop to zero, thus suppressing the impact of noise and faulty data generated by false trust nodes. The preset threshold is determined based on the statistical laws that the normalized residual should follow. Its size corresponds to a high-confidence probability boundary, which statistically distinguishes between normal measurement fluctuations and real outliers that need to be suppressed. When processing the consistency performance of historical interactions between other nodes stored in the historical trust sliding window, an exponentially weighted moving average algorithm is specifically adopted. When calculating the interaction trust value, the residual performance of each interaction in history within the sliding window is given a weight that decays exponentially over time, so as to quickly respond to the latest changes in the state of other nodes.

6. The robot navigation optimization method based on iterative reweighted least squares according to claim 1, characterized in that, The specific implementation of solving the globally optimal navigation constraint weight set through iterative calculation is as follows: the interaction trust value is the initial weight of each edge in the node consensus network. Each weight is iteratively updated. In the (k+1)th iteration, the navigation constraint weight from the self node to other self nodes is determined by a propagation function, which weights and fuses the direct trust of the self node to other self nodes with the indirect evaluation of other self nodes in the network. The input of the function includes at least: the navigation constraint weight between the self node and other self nodes in the kth iteration; and the network consensus support term, which is a weighted aggregation of the navigation constraint weights of all third-party nodes to other self nodes in the cluster. The weight used for aggregation depends on the navigation constraint weight of the self node to third-party nodes. The iterative process continues until the change in all navigation constraint weights in the graph is less than the convergence threshold, reaching an equilibrium state and completing the navigation constraint weight of swarm intelligence.

7. The robot navigation optimization method based on iterative reweighted least squares according to claim 1, characterized in that, The specific implementation of the iterative reweighted least squares algorithm for optimizing the spatiotemporal pose graph containing two types of factors is as follows: the Levenburg-Marquardt algorithm is used as the nonlinear least squares solver. In each iteration, the Levenburg-Marquardt algorithm dynamically switches between the Gauss-Newton method and the gradient descent method by introducing a damping factor.

8. The robot navigation optimization method based on iterative reweighted least squares according to claim 1, characterized in that, The spatiotemporal pose graph is optimized and solved using an iterative reweighted least squares algorithm. The specific implementation process includes: synchronously updating the navigation state of each node in the cluster in each optimization iteration, and recalculating and updating the navigation constraint weights of other nodes based on the updated navigation state until the navigation state converges, thereby obtaining a physically feasible and globally geometrically consistent navigation trajectory solution that has suppressed the influence of false belief nodes. The iterative process is implemented through a nested double loop structure: In the outer loop, the closed-loop residuals of all cross-node tracks are calculated based on the current navigation state, and the navigation constraint weights of all other nodes are updated to form a fixed weight set in this outer loop; In the inner loop, the fixed weight set passed from the outer loop is used to modulate the weighted cross-node closed-loop factors, and the Levenburg-Marquardt algorithm is run for multiple iterations until the spatiotemporal pose map converges under the current weights, obtaining the updated navigation state; The updated navigation state output by the inner loop is used to return to the starting point of the outer loop to begin the next residual calculation and weight update.

Images