Trust adaptive event-driven unscented Kalman fusion filtering mine robot tracking method and system

Abstract

The invention discloses a mine robot tracking method for trust adaptive event-driven unscented Kalman fusion filtering. Firstly, an RSS time-varying response radius-based trust adaptive event-driven anchor point scheduling and anchor point data transmission triggering mechanism is designed to adapt to distribution change scheduling of trust anchor points around a mine robot to be close to a preset number of trust anchor points and dynamically trigger data transmission as required. Then, randomly and uniformly distributed noise covariance is introduced to describe the uncertainty of mine robot motion modeling, and through iterative uniform sampling Sigma point weighted average, the consistency of square root unscented Kalman filter information estimation is ensured; and finally, a credible anchor point separation mechanism of K-means dimensionality reduction clustering is constructed, a credible measurement updating mine robot motion state estimation adaptive weight fusion strategy is constructed, and the accuracy, stability and robustness of mine robot tracking are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of mining robot tracking methods, and in particular to a mining robot tracking method and system using a trust-adaptive event-driven unscented Kalman fusion filter. Background Art

[0002] Mobile wireless sensor networks (MWSNs), as distributed network systems with node mobility, self-organization, and adaptive capabilities, can be flexibly deployed in dynamic environments and adjust perception strategies in real time. They are widely used in scenarios such as mining, geological monitoring, and emergency rescue. However, they lack sufficient response and execution capabilities in complex and dynamic environments. Meanwhile, mobile mining robots (such as inspection robots, detection robots, and transport robots) with strong perception and high computing capabilities have emerged, but their perception capabilities and coverage are limited. Mobile wireless sensor networks, through a large number of dispersed nodes, provide these mobile mining robots with comprehensive environmental information and effective mining robot services.

[0003] In the complex environment of mining areas, due to channel interference, harsh climate, terrain changes and even network attacks, sensor nodes are prone to generate abnormal data, which affects the performance of mining robots. Due to the mobility of mining robots, the motion model is difficult to establish accurately. This modeling uncertainty will make the filtering method unable to accurately adapt to the state changes of the mining robot, thereby affecting the overall tracking performance. Summary of the Invention

[0004] The present invention aims to at least partially address one of the technical problems in the related art. To this end, one objective of the present invention is to propose a mining robot tracking method using a trust-adaptive event-driven unscented Kalman fusion filter. This method adapts to the distribution changes of trust anchor points around the mining robot, schedules an approximately expected number of trust anchor points, and dynamically triggers data transmission on demand. This method achieves the screening and fusion of local estimates of trust nodes, thereby improving the accuracy, stability, and robustness of the mining robot.

[0005] In a first aspect, the present invention proposes a mining robot tracking method using a trust-adaptive event-driven unscented Kalman fusion filter, the method comprising the following steps:

[0006] S1. Initialize the mining robot motion state estimation Set the process noise covariance coefficients d1, d2, d3, d4 and coefficients The sampling interval Preset number of trust response anchor points N e and the initial response radius R rad (0);

[0007] S2. Based on the trust-adaptive event-driven anchor point scheduling and information transmission mechanism algorithm, the surrounding anchor points are scheduled to participate in the mining robot tracking, where the mining robot receives the RSS value and ID measured by the surrounding response anchor points;

[0008] S3. Calculation and prediction of mining robot motion state estimation

[0009]

[0010] Where ΔT represents the sampling time interval, is the estimated motion state of the mining robot predicted at time k, The k-1-time fusion update of the mining robot's motion state estimation;

[0011] S4. Calculate the estimated covariance of the predicted mining robot motion state

[0012]

[0013] Among them, P k-1 / k-1 It is expressed as the estimated covariance of the mining robot motion state updated by fusion at time k-1, is the noise covariance of the process of the hth uniform sampling at time k-1, and H is the number of uniform samplings;

[0014] S5. According to QR decomposition, calculate The square root form

[0015]

[0016] Among them, S k-1 / k-1 P k-1 / k-1 The square root form of

[0017] S6. Calculate multiple Sigma points of the predicted state estimate:

[0018]

[0019] Among them, n x is the state vector dimension, λ is the Sigma point expansion parameter, and the matrix form of multiple Sigma points is:

[0020] S7. Calculate multiple Sigma points for the predicted measurement distance:

[0021] Substituting multiple Sigma points into the nonlinear distance measurement function, we get the predicted distance measurement:

[0022]

[0023] Among them, N r represents the number of response anchor points, is the global position of the i-th response anchor;

[0024] S8. Calculate the predicted measurement distance

[0025]

[0026] Among them, w j is the weight of the traceless Sigma point;

[0027] S9. Calculate the cross-covariance between the estimated motion state of the mining robot and the predicted measured distance:

[0028]

[0029] S10. Calculate the predicted measurement distance covariance and its square root form:

[0030]

[0031] in, The noise covariance measured in response to anchor point i at time k is: QR represents the orthogonal decomposition of a matrix.

[0032] S11. Calculate the robust unscented Kalman filter gain:

[0033]

[0034] S12. Calculate the updated mining robot motion state estimate and its covariance square root form:

[0035]

[0036]

[0037] in, To measure the updated mining robot motion state estimation, The square root form of the covariance of the mining robot's motion state estimation after measurement update;

[0038] S13. Trust cluster G based on K-means dimensionality reduction clustering t Sure:

[0039] G t =C t ∩B t

[0040] Among them, C tRepresents a cluster of response anchors whose state estimation is reliable, B t A cluster of response anchors representing credible state estimate covariances;

[0041] S14. Calculate the mining robot motion state estimation based on clustering fusion update and the square root of their covariance S k / k ;

[0042] G t The nodes in are considered to be trustworthy, and their corresponding state estimation and covariance estimation are as follows:

[0043]

[0044] in, represents the fusion estimation weight obtained according to the trace of the covariance.

[0045] Preferably, in step S2:

[0046] A21, the mining robot sends the response radius R rad The formula for adaptively and dynamically adjusting the surrounding anchor points for a tracking request is as follows:

[0047]

[0048] Among them, R rad represents the response radius, |G t | represents the actual number of trust anchor points, N e Indicates the number of preset expected trust anchor points;

[0049] A22, anchor point i measures and quantifies the RSS value of the request data packet it receives

[0050] A23, Anchor point i judgment Whether it meets the requirements, if it does, it will be scheduled as the response anchor point of the mining robot. The judgment formula is as follows:

[0051]

[0052] A24, anchor point i judgment Whether the requirements are met, if so, the RSS measurement value is sent to the mining robot. The judgment formula is as follows:

[0053]

[0054] in, represents the RSS prior estimate of anchor node i, a represents the pre-filter coefficient, ψ l Indicates the lower limit of the RSS measurement error, ψ u Indicates the upper limit of the RSS measurement error;

[0055] A25. The mining robot receives the RSS values ​​and IDs sent by all response anchors and records the number of current response anchors.

[0056] A26, the mining robot converts the RSS value measured by the anchor node i received into the corresponding measurement distance;

[0057]

[0058] Among them, z ref Indicates the reference distance, y ref Indicates the reference distance z ref The corresponding RSS value, α is the path loss exponent.

[0059] Preferably, in step S8:

[0060]

[0061] Preferably, in step S13:

[0062] A131. Let the state estimation set of all responses to anchor point measurement updates be calculate The difference between the updated prior state estimate and the fusion state estimate is:

[0063]

[0064] Among them, ||·|| represents the 2-norm;

[0065] A132, will Divided into two clusters:

[0066]

[0067] Among them, the mean m (c) Denotes the centers of the two clusters, assuming Indicates allocation Given the index of cluster g, if the mean m (c) Closer Then it's fate otherwise

[0068] A133. To match the sample mean assigned to the cluster data point, the mean is updated as follows:

[0069]

[0070] Repeat steps A132 and A133 until the allocation remains unchanged;

[0071] A134. Compute the state estimation anchor node cluster for trusted measurement updates:

[0072]

[0073] Then g t The updated state of the anchor nodes in the cluster is estimated to be credible, and the cluster is denoted as C t ;

[0074] Similarly, the square root form of the state estimate covariance updated in response to all anchor point measurements is recorded as The column vector set composed of the main diagonal elements of is Calculate the square root form of the prior covariance estimate S k / k-1 The difference between the column vectors formed by the main diagonal elements is denoted as Will Divide into two populations and record them as B t is the set of anchor nodes for the credible estimated covariance estimate;

[0075] Trust cluster G based on K-means dimensionality reduction clustering t Sure;

[0076] G t =C t ∩B t .

[0077] Preferably, in step S14:

[0078]

[0079] In a second aspect, the present invention proposes a mining robot tracking system, comprising an input module, a computing platform module, and an output module:

[0080] The input module is used to input preset data into the computing platform module, and the input module is connected to the input end of the computing platform module;

[0081] The computing platform module applies the above-mentioned trust-adaptive event-driven unscented Kalman fusion filter mining robot tracking method to obtain the cluster fusion updated mining robot motion state estimation and the square root of the covariance S k / k Any one of the options;

[0082] The output module is used to output the result of the operation of the operation platform module, and the signal input end of the output module is connected to the signal output end of the operation platform module.

[0083] The beneficial effects of the present invention are:

[0084] (1) To address the resource constraints of mobile wireless sensor networks, such as channels, power, and computing, a trust-adaptive event-driven anchor scheduling and anchor data transmission triggering mechanism based on the RSS time-varying response radius is designed. This mechanism aims to adapt to the distribution changes of trust anchors around mining robots, schedule an approximately expected number of trust anchors, and dynamically trigger data transmission on demand.

[0085] (2) We constructed a trusted anchor state estimation separation mechanism based on K-means dimensionality reduction clustering, and constructed a state estimation fusion strategy with adaptive fusion weights using covariance, thus achieving the screening and fusion of local estimates of trusted nodes;

[0086] (3) The random uniformly distributed noise covariance is introduced to characterize the uncertainty of the mining robot motion modeling, and the weighted average of the iterative uniformly sampled Sigma points is used to ensure the consistency of the square root unscented Kalman filter information estimation, thereby improving the accuracy, stability and robustness of the mining robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0087] In the attached figure:

[0088] Figure 1 Schematic diagram of the trust-adaptive event-triggered mining robot tracking problem proposed by the present invention;

[0089] Figure 2 Schematic diagram of information exchange between the trusted response anchor point and the untrusted response anchor point proposed in the present invention;

[0090] Figure 3 A comparison chart of the expected number of trust response anchor points and the actual number of trust response anchor points proposed by the present invention;

[0091] Figure 4 This is a comparison chart of the tracking trajectories of the mining robot with q ~ [0.5, 2], q = 4.5, and q = 0.5 proposed in the present invention;

[0092] Figure 5 This is a comparison chart of the tracking errors of the mining robot with q ~ [0.5, 2], q = 4.5, and q = 0.5 proposed in the present invention;

[0093] Figure 6 This is a comparison chart of the tracking trajectories of the mining robot proposed in the present invention with q ~ [0.5, 2], H = 10, Ne = 8;

[0094] Figure 7 This is the tracking error diagram of the mining robot proposed in this invention with q ~ [0.5, 2], H = 10, Ne = 8;

[0095] Figure 8 This is a comparison chart of the tracking trajectories of the mining robots with H=6, H=10, and H=15 proposed in the present invention;

[0096] Figure 9 This is a comparison chart of the tracking errors of the mining robot with H=6, H=10, and H=15 proposed in the present invention;

[0097] Figure 10 This is a comparison chart of the tracking trajectories of the mining robot with Ne=6, Ne=15, and Ne=8 proposed in the present invention;

[0098] Figure 11 This is a comparison chart of the tracking errors of the mining robot proposed in the present invention with Ne=6, Ne=15, and Ne=8;

[0099] Figure 12 This is a comparison chart of the tracking trajectories of the mining robot proposed in the present invention with q=4.5, q~[0.5,2], and q=1.5;

[0100] Figure 13 This is a comparison chart of the tracking errors of the mining robot proposed in the present invention with q=4.5, q~[0.5, 2], and q=1.5. DETAILED DESCRIPTION

[0101] Consider a system composed of N n Mining robots and N f The MWSN system consists of fixed nodes. The mining robot obtains measurement information by triggering fixed nodes. The triggered fixed nodes are called response anchor points, and their number is N. r . These response anchors can send their ID and RSS measurement values ​​to the mining robot. In the actual network operation process, due to the influence of factors such as network attacks, node failures, and signal interference, the measurement data of some response anchors may be abnormal. According to the reliability of the measurement data, the response anchors can be further divided into trusted response anchors and untrusted response anchors. Among them, the trusted response anchor is a node with a known location and reliable measurement, while the untrusted response anchor is a point with a known location but abnormal measurement data. Figure 1 As shown in the figure, a mining robot periodically broadcasts tracking request signals to neighboring nodes through a single-hop communication mechanism and receives data from surrounding anchor points to estimate its own motion state. To reduce communication overhead, energy consumption, and computational burden in the MWSN system, a trust-adaptive event-driven mechanism is needed. This mechanism enables the mining robot to dynamically schedule a preset number of trusted anchor points for measurement based on the distribution of surrounding anchor points and trigger their data transmission on demand, thereby optimizing overall resource utilization. Furthermore, to mitigate the performance degradation of the mining robot caused by modeling errors and anchor point measurement anomalies, an adaptive state fusion estimation algorithm based on K-means dimensionality reduction and two-cluster clustering is further designed to achieve reliable fusion of anchor point measurement information and improve the tracking accuracy and robustness of the system in non-ideal modeling environments.

[0102] 1. System Modeling

[0103] In this embodiment:

[0104] Taking into account the uncertainty of motion modeling, a simplified constant velocity (CV) motion model with an uncertain part ΔF is adopted, that is, the motion equation of the mining robot in the discrete time domain is as follows:

[0105]

[0106] Where:

[0107] Where ΔT is the sampling time interval; x k =[x p (k)x v (k)y p (k)y v (k)] Τ is the motion state of the mining robot at time k, x p (k) and y p (k) is the actual position of the mining robot on the x-axis and y-axis at time k, v (k) and y v (k) is the speed of the mining robot in the x-axis and y-axis directions at time k, is the process noise of the system, assuming that its covariance matrix is The zero-mean Gaussian distribution.

[0108] make If is the unknown uncertain process noise, then Equation (1) can be rewritten as follows:

[0109]

[0110] Let the total process noise w k for:

[0111]

[0112] The covariance of the total process noise is:

[0113]

[0114] Where, is the covariance matrix of the uncertain part of the system, P k / k is the covariance matrix of the estimated motion state at time k. Considering that ΔF is unknown but bounded, we can get:

[0115] δI≤ΔF≤γI (5)

[0116] Where: I is the identity matrix, δ≤γ∈R.

[0117] From formula (5), we can get:

[0118]

[0119] make It is reasonable to assume that obey The uniform distribution of Then Q k obey The uniform distribution is:

[0120] The observation model of the i-th response anchor point is:

[0121]

[0122]

[0123] Where, is the distance between the i-th response anchor node and the mining robot at time k, The covariance is 0 and the mean is The measurement noise, is the RSS measurement value of the response anchor point i at time k, z ref is the reference distance, y ref is the reference distance z ref The corresponding RSS value, α is the path loss index, is the i-th response anchor point on the x-axis, To respond to the position of the anchor point on the y-axis.

[0124] 2. Trust in adaptive event-driven mechanisms

[0125] In this embodiment:

[0126] like Figure 1 As shown, the mining robot will include the response radius R through single-hop communication rad Tracking request packets are broadcast to nearby anchors, R rad It will be dynamically adjusted according to the distribution density of surrounding trust anchor points. Anchor point i measures and quantifies the RSS value of the request data packet it receives. when When (8) is satisfied, it is scheduled as the response anchor point of the mining robot. When formula (9) is also satisfied, the RSS measurement value is Sent to the mining robot.

[0127]

[0128] Where, ψ l Indicates whether the mining robot is within the lower limit of the RSS measurement error, ψ u Indicates the upper limit of the RSS measurement error; is the prior estimate, 0<a<1 is the pre-filter coefficient, R rad is the response radius, N e is the desired number of trust anchors.

[0129] 3. K-means dimensionality reduction clustering robust square root unscented Kalman fusion filter algorithm

[0130] In this example, a random uniformly distributed noise covariance is introduced to describe the uncertainty of motion modeling. A robust square root unscented Kalman fusion filter algorithm is constructed by uniformly random sampling and weighted averaging of multiple sigma points to reduce the adverse effects of uncertainty on mining robot performance. The algorithm includes a prediction step, measurement update, and K-means dimensionality reduction and two-cluster cluster fusion update. Specifically:

[0131] 3.1 Prediction Steps

[0132] Predicting the motion state estimation of mining robots:

[0133]

[0134] The estimated covariance of the predicted motion state is:

[0135]

[0136] Where:

[0137]

[0138] is the estimated motion state of the mining robot predicted at time k, is the k-1-time fusion updated mining robot motion state estimation, P k-1 / k-1 It is expressed as the estimated covariance of the mining robot motion state updated by fusion at time k-1, is the noise covariance of the process at the hth uniform sampling at time k-1, and H is the number of uniform samplings.

[0139] According to QR decomposition, The square root form S k / k-1 as follows:

[0140]

[0141] According to the prior estimate of node i at time k, Multiple Sigma points of the predicted state estimate can be obtained:

[0142]

[0143] Among them, n x is the dimension of the state vector, λ is the parameter for adjusting the distribution of Sigma points, and the matrix form of multiple Sigma points is:

[0144] 3.2 Measurement Update

[0145] Substituting multiple Sigma points into the nonlinear distance measurement function, we get the predicted distance measurement:

[0146]

[0147] Where N r represents the number of response anchor points, is the global position of the i-th response anchor.

[0148] The predicted distance measurement matrix is:

[0149]

[0150] Predicted distance measure:

[0151]

[0152] w j is the weight of the traceless point:

[0153]

[0154] Among them, n x is the state vector dimension, and λ is the Sigma point expansion parameter.

[0155] The cross-covariance between motion state and predicted distance is:

[0156]

[0157] Prediction distance covariance matrix And the square root form is:

[0158]

[0159] in, is the measurement noise covariance of the response anchor point i at time k.

[0160] System state estimates and covariances:

[0161]

[0162]

[0163] Where, To measure the updated mining robot motion state estimation, The square root form of the covariance of the mining robot's motion state estimate after measurement update.

[0164] is the robust square root unscented Kalman filter gain, that is:

[0165]

[0166] 3.3 K-means Dimensionality Reduction Two-Cluster Clustering Fusion Update

[0167] In order to extract the trust set G consisting of reliable information without outlier contamination t The dissimilarity between the calculated state estimate and the fused updated prior state estimate is used as a characteristic indicator for clustering. Untrusted node estimates are ignored, and trusted node estimates are used as an indicator for adaptive weight allocation based on the covariance trace.

[0168] The specific expressions are as follows:

[0169] 1) The state estimation set of all responses to anchor point measurements is recorded as calculate The difference between the updated prior state estimate and the fused state estimate. This paper samples the Euclidean distance to measure the difference between them, as follows:

[0170]

[0171] where ||·|| represents the 2-norm, represents the predicted update of the state estimate of node i after local fusion at time k-1.

[0172] 2) Divided into two clusters:

[0173]

[0174] Among them, the mean m (c) Denotes the centers of the two clusters, assuming Indicates allocation Given the index of cluster g, if the mean m (c) Closer Then it's fate otherwise

[0175] 3) To match the sample mean of the data points assigned to this cluster, the mean is updated as follows:

[0176]

[0177] Repeat steps 2) and 3) until the distribution remains unchanged.

[0178] 4) Compute the state estimation anchor node cluster for trusted measurement updates:

[0179]

[0180] Then g t The updated state of the anchor nodes in the cluster is estimated to be credible, and the cluster is denoted as C t .

[0181] Similarly, the square root form of the state estimate covariance updated in response to all anchor point measurements is recorded as The column vector set composed of the main diagonal elements of is Calculate the square root form of the prior covariance estimate S k / k-1 The difference between the column vectors formed by the main diagonal elements is denoted as Will Divide into two populations and record them as B t is the set of anchor nodes for the credible estimated covariance estimate.

[0182] 5) Trust cluster G based on K-means dimensionality reduction clustering t Sure:

[0183] G t =C t ∩B t (31)

[0184] Then G t The nodes in are considered to be trustworthy, and their corresponding state estimation and covariance estimation are as follows:

[0185]

[0186] in, Represents the fusion estimation weights obtained based on the covariance trace, as follows,

[0187]

[0188] As another embodiment of the present application, this embodiment proposes a mining robot tracking system, including an input module, a computing platform module, and an output module:

[0189] The input module is used to input the preset data into the computing platform module, and the input module is connected to the input terminal of the computing platform module;

[0190] The computing platform module applies the above-mentioned trust-adaptive event-driven unscented Kalman fusion filter mining robot tracking method to obtain the cluster fusion updated mining robot motion state estimation and the square root form of covariance S k / k Any one of the options;

[0191] The output module is used to output the results of the operation of the operation platform module, and the signal input end of the output module is connected to the signal output end of the operation platform module.

[0192] In this embodiment:

[0193] The specific steps of the trust-adaptive event-driven anchor point scheduling and information transmission mechanism are shown in Algorithm 1:

[0194]

[0195] The steps of trust-adaptive event-driven robust square root unscented Kalman fusion filtering for mining robots are shown in Algorithm 2:

[0196]

[0197]

[0198] In order to more clearly illustrate the scheme and effect of this implementation, the following examples are provided with reference to the accompanying drawings:

[0199] Using Matlab software, simulation experiments were conducted using two scenarios for sensor anchor point distribution and mining robot motion trajectory: In one scenario, the sensor anchor points were regularly and evenly distributed in two-dimensional space, with each node spaced 100 degrees apart in both the x- and y-axis directions. The mining robot's actual motion trajectory was a square with a side length of 30 degrees. In the other scenario, the sensor anchor points were randomly distributed in two-dimensional space, and the mining robot's actual motion trajectory was a curved trajectory generated by a polynomial function.

[0200] The initial state estimation and covariance of the mining robot are set as: The process noise covariance is set to Q k =q×I 4×4 , I 4×4 is a 4×4 identity matrix, q~U[q1,q2], where q1=0.5 and q2=2 are the upper and lower bounds of the process noise respectively; the RSS measurement noise covariance is defined as The initial response radius is set to 200.

[0201] In order to analyze the impact of motion modeling uncertainty and the number of expected response trust anchors on the tracking performance of the mining robot in the above scenario, the average error indicator (EEI), the total number of response anchors (TNORA), and the average number of response trust anchors (ANTORA) are defined as follows:

[0202]

[0203] Where C a is the total number of simulation experiments, C m is the total number of simulation experiments, is the number of trust response anchor points at time k in the a-th simulation experiment, x k and are the actual motion state and estimated motion state at time k respectively.

[0204] Table 1 Mining robots TNORA, ANTORA and EEI

[0205]

[0206] like Figure 3 Table 1 shows a comparison of the expected and actual number of trust response anchors. The actual number of trust response anchors triggered by the mining robot during motion is close to the expected value. Compared to a triggering mechanism with a fixed response radius, this mechanism dynamically adjusts the number of participating trust response anchors based on the mining robot's position. While ensuring trust measurement, this avoids redundant anchors from participating in the calculation, effectively reducing data exchange between nodes and significantly lowering the communication burden, thus facilitating the implementation of real-time mining robots.

[0207] like Figure 4-5 As shown in Figures 8-9, 10-11, and 12-13, the tracking trajectory of the mining robot is closer to the true trajectory when q ~ [0.5, 2], H = 10, and Ne = 8. The EEI of the robust unscented Kalman fusion filter with trust-adaptive event-driven filtering deteriorates when the uncertainty characterized by process noise is overestimated or underestimated. When the process noise used in the estimator is underestimated, the estimator overtrusts the predicted state under the uncertainty of motion modeling and deviates from the actual state; when the covariance of the process noise is overestimated, the estimator overtrusts the measured value and ignores the prior information of the motion model. Further comparisons with traditional root mean square UKF and non-root mean square UKF show that the proposed method is highly robust in dealing with these uncertainties.

[0208] Figure 6-7 As shown in the figure, when the number of trust response anchors is 8, the mining robot's tracking performance is better than the tracking results with other numbers of trust response anchors. Mining robots typically rely on measurement data from multiple anchors for triangulation. When the number of trust response anchors is too small, the provided set constraints are insufficient, resulting in a decrease in the mining robot's accuracy. When the number of trust response anchors is too large, the computational, power, and communication burdens on the MWSN nodes are increased.

[0209] In summary, this paper proposes a trust-adaptive event-driven unscented Kalman fusion filter algorithm for resource-limited MWSNs to address the impact of motion modeling uncertainty and node failures on mining robot tracking performance. In the proposed algorithm, a trust-adaptive event-driven mechanism based on the time-varying response radius of the RSS effectively controls the number of triggered trust anchors, reducing the energy, computational, and communication burdens associated with redundant data transmission. Furthermore, a robust square root unscented Kalman fusion filter algorithm based on K-means dimensionality reduction clustering is constructed. This algorithm uses weighted averaging of uniform random sampling of multiple sigma points to minimize the impact of uncertainty on mining robot performance. A state estimation fusion strategy with adaptive weights is constructed using covariance to filter and fuse local estimates of trust anchors, thereby improving the accuracy, stability, and robustness of mining robot tracking.

Claims

1. A mining robot tracking method based on trust-adaptive event-driven unscented Kalman fusion filtering, characterized in that: The method steps are as follows: S1. Initialize the mining robot motion state estimation Set the process noise covariance coefficients d1, d2, d3, d4 and coefficients The sampling interval Preset number of trust response anchor points N e and the initial response radius R rad (0); S2. Based on the trust-adaptive event-driven anchor point scheduling and information transmission mechanism algorithm, the surrounding anchor points are scheduled to participate in the mining robot tracking, where the mining robot receives the RSS value and ID measured by the surrounding response anchor points; S3. Calculation and prediction of mining robot motion state estimation Where ΔT represents the sampling time interval, is the estimated motion state of the mining robot predicted at time k, The k-1-time fusion update of the mining robot's motion state estimation; S4. Calculate the estimated covariance of the predicted mining robot motion state Among them, P k-1 / k-1 It is expressed as the estimated covariance of the mining robot motion state updated by fusion at time k-1, is the noise covariance of the process of the hth uniform sampling at time k-1, and H is the number of uniform samplings; S5. According to QR decomposition, calculate The square root form Among them, S k-1 / k-1 P k-1 / k-1 The square root form of S6. Calculate multiple Sigma points of the predicted state estimate: Among them, n x is the state vector dimension, λ is the Sigma point expansion parameter, and the matrix form of multiple Sigma points is: S7. Calculate multiple Sigma points for the predicted measurement distance: Substituting multiple Sigma points into the nonlinear distance measurement function, we can get the predicted measured distance: Among them, N r represents the number of response anchor points, is the global position of the i-th response anchor; S8. Calculate the predicted measurement distance Among them, w j is the weight of the traceless Sigma point; S9. Calculate the cross-covariance between the estimated motion state of the mining robot and the predicted measured distance: S10. Calculate the predicted measurement distance covariance and its square root form: in, The noise covariance measured in response to anchor point i at time k is: QR represents the orthogonal decomposition of a matrix. S11. Calculate the robust unscented Kalman filter gain: S12. Calculate the updated mining robot motion state estimate and its covariance square root form: in, To measure the updated mining robot motion state estimation, The square root form of the covariance of the mining robot's motion state estimation after measurement update; S13. Trust cluster G based on K-means dimensionality reduction clustering t Sure: G t =C t ∩B t Among them, C t Represents a cluster of response anchors whose state estimation is reliable, B t A cluster of response anchors representing credible state estimate covariances; S14. Calculate the mining robot motion state estimation based on clustering fusion update and the square root of its covariance S k / k ; G t The nodes in are considered to be trustworthy, and their corresponding state estimation and covariance estimation are as follows: in, represents the fusion estimation weight obtained according to the trace of the covariance.

2. The mining robot tracking method based on trust-adaptive event-driven unscented Kalman fusion filtering according to claim 1 is characterized in that: In step S2: A21, the mining robot sends the response radius R rad The formula for adaptive dynamic scheduling of surrounding anchor points for tracking requests is as follows: Among them, R rad represents the response radius, |G t | represents the actual number of trust anchor points, N e Indicates the number of preset expected trust anchor points; A22, anchor point i measures and quantifies the RSS value of the request data packet it receives A23, Anchor point i judgment Whether it meets the requirements, if it does, it will be scheduled as the response anchor point of the mining robot. The judgment formula is as follows: A24, anchor point i judgment Whether the requirements are met, if so, the RSS measurement value is sent to the mining robot. The judgment formula is as follows: in, represents the RSS prior estimate of anchor node i, a represents the pre-filter coefficient, ψ l Indicates the lower limit of the RSS measurement error, ψ u Indicates the upper limit of the RSS measurement error; A25. The mining robot receives the RSS values ​​and IDs sent by all response anchors and records the number of current response anchors. A26, the mining robot converts the RSS value measured by the anchor node i received into the corresponding measurement distance; Among them, z ref Indicates the reference distance, y ref Indicates the reference distance z ref The corresponding RSS value, α is the path loss exponent.

3. The mining robot tracking method based on trust-adaptive event-driven unscented Kalman fusion filtering according to claim 1 is characterized in that: In step S8:

4. The mining robot tracking method based on trust-adaptive event-driven unscented Kalman fusion filtering according to claim 1 is characterized in that: In step S13: A131. Let the state estimation set of all responses to anchor point measurement updates be calculate The difference between the updated prior state estimate and the fusion state estimate is: Among them, ||·|| represents the 2-norm; A132, will Divided into two clusters: Among them, the mean m (c) Denotes the centers of the two clusters, assuming Indicates allocation Given the index of cluster g, if the mean m (c) Closer Then it's fate otherwise A133. To match the sample mean assigned to the cluster data point, the mean is updated as follows: Repeat steps A132 and A133 until the allocation remains unchanged; A134. Compute the state estimation anchor node cluster for trusted measurement updates: Then g t The updated state of the anchor nodes in the cluster is estimated to be credible, and the cluster is denoted as C t ; Similarly, the square root form of the state estimate covariance updated in response to all anchor point measurements is recorded as The column vector set composed of the main diagonal elements of is Calculate the square root form of the prior covariance estimate S k / k-1 The difference between the column vectors formed by the main diagonal elements is denoted as Will Divide into two populations and record them as B t is the set of anchor nodes for the credible estimated covariance estimate; Trust cluster G based on K-means dimensionality reduction clustering t Sure; G t =C t ∩B t 。 5. The mining robot tracking method based on trust-adaptive event-driven unscented Kalman fusion filtering according to claim 1 is characterized in that: In step S14:

6. A mining robot tracking system, characterized by: Including input module, computing platform module and output module: The input module is used to input preset data into the computing platform module, and the input module is connected to the input end of the computing platform module; The computing platform module applies the mining robot tracking method of trust-adaptive event-driven unscented Kalman fusion filtering as described in any one of claims 1-6 to obtain the cluster fusion updated mining robot motion state estimation and the square root form of covariance S k / k ; The output module is used to output the result of the operation of the operation platform module, and the signal input end of the output module is connected to the signal output end of the operation platform module.

Images