UWB positioning-based agricultural distribution network operation track robot autonomous following system

Abstract

The application relates to the technical field of robot autonomous navigation, and discloses a track robot autonomous following system for agricultural distribution network operation based on UWB positioning, which acquires a current channel state through a channel state identification module and calculates corresponding measurement noise covariance by a dynamic covariance reconstruction module; a state estimation module dynamically adjusts the trust weight of UWB measurement values by applying the covariance, thereby realizing passive adaptation to bad data. Meanwhile, the system predicts future signal quality through a channel state prediction module, and a path planning module takes the prediction result as a path evaluation cost to actively avoid entering a signal quality deterioration area. The application adopts a cooperative control strategy combining active avoidance and passive adaptation, realizes double protection of physical path adjustment and algorithm level self-adaptation, and improves the robustness, continuity and operation safety of the track robot in autonomous following in a complex environment.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation technology for robots, specifically to an autonomous following system for tracked robots used in agricultural power distribution network operations based on UWB positioning. Background Technology

[0002] Tracked robots are used in material transportation and collaborative tasks in agricultural power grid operation environments due to their good adaptability to complex terrain. To enable robots to automatically follow workers or equipment, a high-precision real-time positioning system is usually required; ultra-wideband (UWB) positioning technology has become a commonly used technical solution in this application scenario due to its high accuracy and ability to penetrate obstacles to a certain extent.

[0003] However, the operating environment of rural power distribution networks is typically complex, with various obstacles such as utility poles, trees, and fences. These obstacles can block the straight-line propagation path of the signal between the UWB beacon and the receiver, leading to non-line-of-sight (NLS) propagation. Under NLS propagation conditions, the measured values ​​of the UWB signal will deviate significantly, and the reliability of the positioning results will decrease drastically. Existing autonomous following systems often lack effective prediction and adaptive mechanisms when dealing with this type of signal quality degradation problem. This can cause the robot to experience drastic changes or temporary loss of positioning results when encountering signal obstruction, leading to unstable following behavior such as sudden stops, getting lost, or path deviations, affecting the continuity and safety of the operation.

[0004] Therefore, this invention proposes an autonomous following system for tracked robots operating agricultural power distribution networks based on UWB positioning to address the shortcomings of existing technologies. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides an autonomous following system for tracked robots operating agricultural power distribution networks based on UWB positioning, which solves the problem of unstable positioning of tracked robots in complex environments due to non-line-of-sight signal propagation.

[0006] To achieve the above objectives, the present invention provides the following technical solution: an autonomous following system for a tracked robot operating agricultural power distribution networks based on UWB positioning, comprising: a UWB beacon associated with the target to be followed; a UWB receiver mounted on the tracked robot; and a control system associated with the UWB receiver; the control system is configured to perform the following operation: acquiring channel impulse response data corresponding to the signal emitted by the UWB beacon from the UWB receiver; the control system includes:

[0007] The channel state identification module is used to identify the current channel state, which characterizes the current signal propagation path state, based on the channel impulse response data.

[0008] A state estimation module, which includes a state estimation filter, is used to fuse UWB distance measurements and robot body sensor data to estimate the motion state of the tracked robot relative to the target to be followed.

[0009] The control system is further configured to dynamically adjust one or more model parameters of the state estimation filter based on the current channel state.

[0010] Preferably, the model parameters of the state estimation filter include the measurement noise covariance corresponding to the UWB distance measurement;

[0011] The control system is configured to dynamically adjust the value of the measurement noise covariance based on the current channel state.

[0012] Preferably, the channel state identification module is configured to extract at least one channel feature from the channel impulse response data, wherein the channel feature is selected from the root mean square delay spread, the power ratio of the first path component and the strongest path component, and to identify the current channel state based on the channel feature.

[0013] Preferably, the control system further includes a dynamic covariance reconstruction module, which is configured to use fuzzy inference logic to establish a mapping relationship between the channel feature quantity and the measurement noise covariance, and to calculate the real-time value of the channel feature quantity into a continuously changing measurement noise covariance value according to the mapping relationship.

[0014] Preferably, the control system further includes a channel state prediction module and a path planning module;

[0015] The channel state prediction module is used to predict the future channel state based on the historical time series of channel impulse response data.

[0016] The path planning module is configured to adjust the travel path of the tracked robot based on the predicted channel state.

[0017] Preferably, the path planning module is configured to generate a dynamic signal cost map based on the predicted channel state, wherein the cost value in the dynamic signal cost map is inversely proportional to the predicted signal propagation quality, and the dynamic signal cost map is used as a cost item in the path planning evaluation function.

[0018] Preferably, the path planning evaluation function also integrates an obstacle avoidance cost term for evaluating the distance to physical obstacles and a heading cost term for evaluating the heading deviation from the target to be followed, in order to generate a comprehensive evaluation result.

[0019] Preferably, the channel state identification module is further configured to automatically collect channel impulse response data in a stable motion state as a high-confidence line-of-sight sample when the tracked robot is in a preset stable motion state, and use the high-confidence line-of-sight sample to perform online self-calibration of the current channel state identification logic.

[0020] Preferably, the control system further includes an autonomous following mode controller, which is configured to convert the value of the dynamically adjusted measurement noise covariance into a quantified following confidence index, and automatically switch between a standard autonomous following mode, a deceleration cautious following mode, and a safe hovering mode based on the value of the following confidence index.

[0021] Preferably, the control system is configured as a collaborative path planning module and a state estimation module to proactively avoid a future signal quality degradation by adjusting the driving path through the path planning module, and to ensure the stability of the state estimation by dynamically adjusting the model parameters of the state estimation filter when the predicted channel state indicates a degradation in the signal quality.

[0022] This invention provides an autonomous following system for a tracked robot operating in agricultural power distribution networks based on UWB positioning. It has the following beneficial effects:

[0023] 1. This invention incorporates predicted signal propagation quality information into the path evaluation function by setting up a channel state prediction module and a path planning module. This allows the tracked robot to plan and select a travel path that maintains good communication quality in advance. This proactive avoidance strategy prevents positioning interruptions or severe data quality degradation caused by the robot entering a signal obstruction area. Compared to technologies that only react after a problem occurs, this invention ensures the continuity and smoothness of the autonomous following task.

[0024] 2. This invention establishes a direct mapping relationship between the current channel state and the UWB measurement noise covariance by setting up a dynamic covariance reconstruction module and a state estimation module. When the signal propagation quality deteriorates, the system automatically increases the value of the measurement noise covariance, thereby reducing the trust weight of the state estimation filter on the unreliable measurement value. This passive adaptive mechanism can effectively suppress the impact of abnormal measurement data on the final positioning result in non-line-of-sight environments, ensuring that the output motion state estimate remains stable and reliable even when the signal quality is poor.

[0025] 3. This invention, by setting up an autonomous following mode controller, converts the measurement noise covariance output by the dynamic covariance reconstruction module into a quantified following confidence index, and automatically switches between various operating modes such as standard following, decelerated following, and safe hovering based on this index. This design directly links the reliability of the underlying signal with the behavior strategy of the top-level system, ensuring that the tracked robot can automatically take safety measures such as deceleration or stopping when the positioning information is unreliable, thereby significantly improving the safety and mission success rate of the entire system in complex agricultural power grid environments. Attached Figure Description

[0026] Figure 1 This is a schematic diagram of the functional modules of the present invention;

[0027] Figure 2 This is a schematic diagram illustrating an implementation method of the present invention based on fuzzy logic;

[0028] Figure 3 This is a diagram showing the operating mode state transition of the autonomous following mode controller of the present invention;

[0029] Figure 4 This is a schematic diagram illustrating an application scenario of the active avoidance strategy of the present invention.

[0030] Among them, 10 is the channel state identification module; 20 is the channel state prediction module; 30 is the path planning module; 40 is the dynamic covariance reconstruction module; 50 is the state estimation module; and 60 is the autonomous following mode controller. Detailed Implementation

[0031] The technical solutions in 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.

[0032] See attached document Figure 1 , Figure 1 This is a functional module diagram according to an embodiment of the present invention. The present invention provides an autonomous following system for a tracked robot in agricultural power distribution network operations based on UWB positioning. The system is applied to the tracked robot body, which integrates hardware including a central processing unit, a memory, and multiple sensors electrically connected to the central processing unit. The sensors include: an inertial measurement unit (IMU) for acquiring the robot's angular velocity and acceleration, a wheel speed encoder for acquiring the robot's travel distance, and a lidar for sensing surrounding physical obstacles.

[0033] The system hardware of the present invention also includes a UWB beacon associated with the personnel or equipment to be followed, and a UWB receiver mounted on the tracked robot; the UWB receiver is used to receive the signal emitted by the UWB beacon and to parse it to obtain the UWB distance measurement value and channel impulse response data.

[0034] The system's control functions are implemented by software modules running on the central processing unit; see appendix. Figure 1 The control system includes: a channel state identification module 10, a channel state prediction module 20, a path planning module 30, a dynamic covariance reconstruction module 40, a state estimation module 50, and an autonomous following mode controller 60.

[0035] In this embodiment of the invention, the overall workflow of the autonomous following system within one control cycle is as follows:

[0036] First, the control system synchronously acquires data from various hardware units through its interfaces. This includes: UWB distance measurements and raw channel impulse response data from the UWB receiver; robot motion data from the inertial measurement unit and wheel speed encoder; and point cloud data of the surrounding environment from the lidar.

[0037] After acquiring the data, the channel state identification module 10 and the channel state prediction module 20 process the channel impulse response data in parallel. The channel state identification module 10 analyzes and outputs the current channel state, which represents the current signal propagation path state, based on the channel impulse response data of the current frame. At the same time, the channel state prediction module 20 performs trend analysis based on the historical time series composed of channel impulse response data from multiple historical frames, and outputs the predicted channel state, which represents the signal propagation quality in the short term.

[0038] The path planning module 30 receives the predicted channel state output by the channel state prediction module 20; the path planning module 30 converts the predicted channel state into a dynamic signal cost map, and combines it with the physical obstacle information obtained from the lidar, and performs a comprehensive evaluation in its path evaluation function to plan or adjust a driving path that can actively avoid areas where the signal quality will deteriorate in the future.

[0039] The dynamic covariance reconstruction module 40 receives the current channel state output by the channel state identification module 10; based on the current channel state, the dynamic covariance reconstruction module 40 calculates a measurement noise covariance value that is directly related to the current signal propagation quality in real time.

[0040] The state estimation module 50 receives the measurement noise covariance value output by the dynamic covariance reconstruction module 40. In a state estimation filter, the state estimation module 50 first performs state prediction using the robot body motion data, then performs state update using the UWB distance measurement value and the dynamically adjusted measurement noise covariance value, and finally outputs the corrected and highly stable estimated values ​​of the robot pose and motion state.

[0041] The robot's underlying motion controller generates and issues control commands to drive the tracks based on the travel path output by the path planning module 30 and the real-time state estimation value output by the state estimation module 50.

[0042] In addition, the autonomous follow mode controller 60 receives the measurement noise covariance value output by the dynamic covariance reconstruction module 40; the autonomous follow mode controller 60 uses it as a quantitative confidence index and switches between preset multiple operating modes based on the value of the index to adjust the overall operating strategy of the system.

[0043] To further clarify the implementation of each technical aspect of the present invention, the following will provide a detailed description of the implementation of each functional module involved above and its internal processing flow.

[0044] The channel state identification module 10 is used to process and analyze the raw channel impulse response data output by the UWB receiver in order to identify the current channel state that can accurately reflect the current propagation path state of the UWB signal in the physical space. The specific implementation process of this module is as follows.

[0045] First, the channel state identification module 10 receives the current time. Channel impulse response data acquired by the physical layer of the UWB receiver The data is a function describing the changes in amplitude and phase of a signal over time after it travels through multiple different paths to the receiver. The channel state identification module 10 extracts a set of channel features that can quantify the characteristics of channel propagation from the raw data through the following steps.

[0046] S211, Calculate the root mean square time delay spread of the signal. This characteristic is used to characterize the richness and temporal dispersion of the signal's multipath components; its calculation formula is: ;

[0047] in, For time delay, The average additional delay at the current moment is calculated using the following formula: ;

[0048] In practical discrete-time systems, the above integration operation is replaced by a summation operation; a large root mean square delay spread usually means that the signal has undergone more reflections or diffractions, and the multipath effect is significant, which is a typical characteristic of non-line-of-sight propagation environments.

[0049] S212, calculate the power ratio of the first path component to the strongest path component of the signal. This characteristic is used to determine the availability and strength of the signal along the direct path; its calculation method is as follows: first, in the channel impulse response data... The power of the first arrival path is identified in the middle. and the maximum power of all paths Then calculate the ratio of the two: ;

[0050] In line-of-sight propagation environments, the first path of arrival is the shortest direct path, and its energy is usually the strongest, so the power ratio is close to 1. In non-line-of-sight environments, the direct path is severely attenuated by obstacles, and its energy may be much less than that of a strong reflection path, resulting in a power ratio that is significantly less than 1.

[0051] The channel state identification module 10 extracts at least the root mean square delay spread and the power ratio of the first path component to the strongest path component, and constructs a feature vector. Then, based on the feature vector, the channel state identification module 10 classifies the current channel state through a set of preset classification logic.

[0052] In one specific embodiment, the classification logic is implemented through a set of threshold judgment rules. The channel state identification module 10 compares the calculated channel feature quantities with preset thresholds. For example, for the root mean square delay spread... Set low threshold and high threshold , is the power ratio Set a high threshold and low threshold .

[0053] Classification logic may include: if and If the current channel state is determined to be a strong line-of-sight state, this condition corresponds to the ideal propagation situation where the multipath effect is weak and the direct path energy is the strongest.

[0054] like or If the current channel state is determined to be a hard non-line-of-sight state, this condition corresponds to a situation where the multipath effect is very severe, or the propagation energy of the direct path is much less than that of the reflected path.

[0055] Other combinations that do not meet the above two conditions are classified as an intermediate state between weak line of sight and soft non-line of sight.

[0056] Through the above steps, the channel state identification module 10 finally outputs a discrete label representing the current channel state. This label is then sent to the dynamic covariance reconstruction module 40 as the direct basis for adjusting the measurement noise covariance. As for the threshold used in the classification logic, the specific value setting method is a well-known technology in the field and can be obtained through offline dataset calibration or online self-calibration, etc., which will not be elaborated here.

[0057] To improve the adaptability of the channel state identification module 10 to different agricultural power distribution network operation environments and overcome the misjudgment problem that may occur in a variable environment due to a fixed classification threshold, the channel state identification module 10 further includes an online self-calibration mechanism. The implementation of this mechanism is to use the motion state information of the tracked robot itself to autonomously identify a high-confidence line-of-sight propagation scenario, and use the channel data collected in the scenario to dynamically and online calibrate the classification logic of the current channel state. The specific implementation steps of this mechanism are as follows.

[0058] S213, determine whether the tracked robot is in a preset stable motion state; the channel state identification module 10 continuously receives angular velocity data from the inertial measurement unit on the robot body. and linear acceleration data When angular velocity data The absolute value is less than the preset angular velocity stability threshold for a certain period of time. And linear acceleration data The absolute value is less than the preset acceleration stabilization threshold during this duration. If the robot is in a stable state of approximately uniform linear motion, then it is determined that the robot is currently in a stable state of motion. ;

[0059] in, The angular velocity stability threshold, This is the acceleration stabilization threshold; this stable motion state is the trigger condition for subsequent operations.

[0060] S214, Collect high-confidence line-of-sight samples; when determining that the robot is in a stable motion state, the system is based on a basic assumption that when the robot is performing smooth motion without significant acceleration, deceleration, or turning, it is highly likely to maintain an unobstructed line-of-sight propagation path with the target it is following; therefore, the channel state identification module 10 automatically collects the channel characteristic quantities at this moment, including the root mean square delay spread. The power ratio of the first path component to the strongest path component The channel state identification module 10 collects multiple high-confidence line-of-sight samples and stores them in a sample buffer pool with first-in-first-out characteristics.

[0061] S215, update the baseline threshold of the classification logic; the channel state identification module 10 uses one or more high-confidence line-of-sight samples from the sample buffer pool to update the baseline threshold used in the current channel state classification logic online; in a specific embodiment, the update method is as follows:

[0062] Power ratio threshold used to determine strong line-of-sight conditions Perform an update; calculate the average power ratio of all samples in the sample buffer. And adjust the threshold based on this average value: ;in, The default scaling factor is less than 1, for example, 0.9.

[0063] For the root mean square delay spread threshold used to determine strong line-of-sight conditions Perform an update; calculate the average root mean square delay spread of all samples in the sample buffer pool. and standard deviation And adjust the threshold based on these two statistics: ;in, This is a preset multiplier, such as 1.0 or 1.5.

[0064] The purpose of the channel state prediction module 20 is to predict the signal propagation state in the short term based on the changing trends of historical channel characteristics, and to provide the prediction result to the path planning module 30 so as to actively avoid areas of signal deterioration; the specific implementation steps of this module are as follows.

[0065] S221, maintaining the historical time series of channel features; the channel state prediction module 20 internally has a first-in-first-out data buffer of length N, used to store the channel features extracted by the channel state identification module 10 in the past N consecutive control cycles; this constitutes one or more historical time series, for example, the root mean square delay spread time series. .

[0066] S222, perform trend analysis on the historical time series; for the time series of each channel feature, the channel state prediction module 20 uses the least squares method for linear fitting to calculate the current rate of change; taking the root mean square delay spread as an example, for N data points in the time series... ,in For time indexing, For the corresponding feature values, this module fits a straight line. Among them, the slope This refers to the rate of change of the feature quantity within the current time window, and its calculation formula is: ;intercept The calculation formula is: ;

[0067] S223, predict future channel characteristic values; based on this linear fitting model, the channel state prediction module 20 performs forward extrapolation to predict future... After a time step, that is, at Channel eigenvalues ​​at time t; predicted root mean square delay spread The calculation is as follows:

[0068] ;

[0069] in, The preset prediction step size is used; the same prediction process is also performed on other channel features, including the power ratio of the first path component and the strongest path component, to obtain a complete set of predicted channel feature values.

[0070] S224, determine the predicted channel state; the channel state prediction module 20 inputs a set of predicted channel feature values ​​obtained in S223 into the classification logic that is the same as the current channel state classification logic used by the channel state identification module 10; by comparing with the preset or online calibrated threshold, the corresponding predicted channel state is determined, for example, predicted as strong line-of-sight, hard non-line-of-sight, etc.

[0071] Finally, the channel state prediction module 20 outputs the predicted channel state to the path planning module 30. Through the above steps, the system can detect in advance the signal blockage or multipath effect aggravation that may be caused by the movement of the robot or the target personnel, providing a basis for decision-making for subsequent active path adjustment. For the least squares linear fitting used in the trend analysis, those skilled in the art can replace it with other time series prediction models such as Kalman filter prediction and autoregressive model (AR model). The selection and implementation of these models are well-known technologies in the field and will not be described in detail here.

[0072] The path planning module 30 is used to plan the travel path of the tracked robot. In order to actively avoid areas with deteriorated signal, the path planning module 30 receives the predicted channel state from the channel state prediction module 20 and generates a dynamic signal cost map based on the prediction result. The method for generating the map may specifically include the following steps.

[0073] S231, the discrete predicted channel state is mapped to a continuous predicted signal quality index; the path planning module 30 receives the predicted channel state label, characterized by the future signal propagation state, output by the channel state prediction module 20, such as strong line-of-sight, weak line-of-sight, or hard non-line-of-sight; the path planning module 30 converts these discrete labels into a standardized, continuous scalar value, i.e., the predicted signal quality index, through a preset mapping rule. In one embodiment, the mapping rule can be set as follows: if the predicted channel state is strong line-of-sight, then The value is assigned to 1.0; if it is a weak line of sight, the value is assigned to 0.5; if it is a hard non-line of sight, the value is assigned to 0.0.

[0074] S232, the predicted signal quality index is converted into a signal cost value that is inversely proportional to the predicted signal propagation quality; in order for the path planning algorithm to directly utilize this information, the path planning module 30 uses a preset mapping function to convert the predicted signal quality index... Convert to signal cost value This mapping function ensures that when the predicted signal quality is high, the corresponding cost is low, and vice versa. In a specific embodiment, this mapping function can take the form of a sigmoid function:

[0075] ;

[0076] in: This represents the maximum signal cost, and is a configurable normal value. This is the gain coefficient, used to adjust the steepness of the cost curve; This is the offset coefficient, used to set the center point of the sensitive interval for signal quality indicators.

[0077] Through this function, a low The value will lead to a high The value, and vice versa, thus achieving an inverse mapping relationship between the cost value and the predicted signal propagation quality.

[0078] S233, Generate a dynamic signal cost map; this dynamic signal cost map is not a global static map covering the entire work area, but a dynamic cost layer applied to a local path planner (e.g., Dynamic Window Method (DWA)) when evaluating candidate trajectories; in each planning cycle, the path planning module 30 generates multiple candidate driving trajectories; for each candidate trajectory, the system calculates the endpoint position of the trajectory after the prediction step; since the signal quality at the endpoint position has been predicted, the system uses the signal cost value calculated in S232 associated with the endpoint position. The attributes of the trajectory are attached to it; the set of all candidate trajectories and their corresponding signal cost values ​​logically constitutes a dynamic signal cost map centered on the robot and updated in real time as the robot moves.

[0079] The dynamic signal cost map is then used as a cost item in the path planning evaluation function of the path planning module 30, and is weighted and fused with other cost items (such as obstacle avoidance cost, heading cost, etc.) so that when selecting a path, the tracked robot tends to avoid those directions of travel that will lead to a decline in future signal quality.

[0080] The path planning module 30 operates within the framework of a local path planning algorithm, such as the Dynamic Window (DWA) method. The basic working principle of the DWA algorithm—namely, sampling within a velocity space that satisfies robot dynamics constraints and evaluating multiple candidate trajectories corresponding to the sampled velocities to select the optimal one—is itself well-known in the field and will not be elaborated upon here. The core improvement of this invention lies in the implementation of its path evaluation function.

[0081] In each planning cycle, for each pair of sampling rates The determined candidate driving trajectories are evaluated using a multi-objective fusion path evaluation function by the path planning module 30. The function evaluates the trajectory; its purpose is to quantify the quality of the trajectory from multiple dimensions and ultimately select the speed command corresponding to the trajectory with the highest total score. The specific form of this path evaluation function is as follows:

[0082] ;

[0083] Among them, the function The components are defined as follows:

[0084] S234, Calculation of heading cost item; This term is used to evaluate the deviation of the candidate trajectory's endpoint orientation from the heading of the currently followed target; the value is typically 180 degrees minus the angle between the trajectory endpoint's heading and the target's direction; a smaller heading deviation will result in a higher value. Score.

[0085] S235, Calculation of obstacle avoidance cost; This item is used to evaluate the distance between the candidate trajectory and the nearest physical obstacle; this distance information comes from the environmental perception data of the LiDAR; if the minimum distance between the trajectory and the obstacle is less than a preset safety threshold, then the trajectory's... A score of zero is discarded; otherwise, the score is proportional to the minimum distance.

[0086] S236, Calculation of speed cost item; This item evaluates the magnitude of the linear velocity corresponding to the candidate trajectory; in autonomous following tasks, robots are typically encouraged to maintain a high linear velocity that matches the target's movement speed, while ensuring safety. Therefore, this item's score is related to the linear velocity. The size is directly proportional to the size.

[0087] S237, Calculation of signal cost item; The term refers to the signal cost value generated in the aforementioned steps and associated with the endpoint of the candidate trajectory. To unify the optimization direction of the evaluation function (higher score, better), here... The term actually uses the reciprocal of the signal cost value or a function that decreases as the signal cost value increases; for example, .

[0088] In the above formula, , , and These are the preset weighting coefficients used to adjust the relative importance between different evaluation objectives; It is a normalization function used to scale the scores of different physical quantities to a comparable scale before performing a weighted summation.

[0089] Path planning module 30 calculates all valid candidate trajectories. Total score, and select those that make The velocity pair corresponding to the trajectory with the largest value This serves as the final control command output to the robot's underlying motion controller. In this way, signal propagation quality is incorporated into the path decision-making process as an independent optimization objective, ensuring that the system selects a path that is the optimal solution for both physical safety and communication quality.

[0090] See attached document Figure 2 , Figure 2 yes Figure 1 A schematic diagram of a fuzzy logic-based implementation of the dynamic covariance reconstruction module. The dynamic covariance reconstruction module 40 is responsible for establishing a mapping relationship between the current channel state and the measurement noise covariance corresponding to the UWB distance measurement value in the state estimation module 50. Based on this relationship, the channel feature quantity output by the channel state identification module 10 is calculated into a continuously changing measurement noise covariance value.

[0091] In a preferred embodiment, the dynamic covariance reconstruction module 40 is implemented as a controller based on fuzzy inference logic, and its internal processing flow may specifically include the following steps:

[0092] S241, the input channel feature quantity is fuzzified; the dynamic covariance reconstruction module 40 receives the precise channel feature quantity, such as root mean square delay spread, output by the channel state identification module 10, which characterizes the current signal propagation state. The power ratio of the first path component to the strongest path component The dynamic covariance reconstruction module 40 uses a preset membership function to convert these precise numerical inputs into multiple fuzzy linguistic variables with different membership degrees; for example, it converts... The domain of discourse is divided into three fuzzy sets: "small", "medium", and "large". The universe of discourse is divided into three fuzzy sets: "small", "medium", and "large"; the output variable is the measurement noise covariance. Its domain is divided into five fuzzy sets: “very small”, “small”, “medium”, “large”, and “very large”.

[0093] S242, fuzzy inference based on a fuzzy rule base; the dynamic covariance reconstruction module 40 has an internal fuzzy rule base in the form of "IF-THEN" based on expert knowledge presets; these rules establish the logical relationship between input fuzzy variables and output fuzzy variables; specific rules may include:

[0094] Rule 1: If For "big" or If it is "small", then it is inferred that... The value is "very large"; this rule corresponds to situations where there is severe multipath in the signal propagation path or the direct path is severely blocked, in which case the unreliability of the UWB measurement value is the highest.

[0095] Rule 2: If For "small" and If it is "large", then it can be inferred that... The value is "very small"; this rule corresponds to a clear line-of-sight propagation of the signal, and UWB measurements have a high degree of reliability.

[0096] Rule 3: If For "middle" and If it is "middle", then it is inferred that... It is rated as "medium";

[0097] By performing parallel reasoning on all rules and aggregating the reasoning results of each rule, a final fuzzy output set is obtained.

[0098] S243, perform defuzzification calculation on the fuzzy output result; to obtain an accurate value that can be used for subsequent mathematical operations, the dynamic covariance reconstruction module 40 performs defuzzification calculation on the fuzzy output set aggregated in S242; in a specific embodiment, the defuzzification calculation adopts the centroid method; the calculation formula is as follows:

[0099] ;

[0100] in: It is the numerical value of the precise measurement noise covariance obtained after defuzzification; It is a continuous variable within the domain of the output variable (measurement noise covariance); It is the membership function of the aggregated output fuzzy set.

[0101] Through the above steps, the dynamic covariance reconstruction module 40 smoothly and non-linearly maps the feature quantities characterizing the physical channel quality to an accurate measurement noise covariance value. The value is then transmitted to the state estimation module 50 to dynamically adjust the model parameters of its state estimation algorithm. The specific form of the membership function in the fuzzy logic controller (such as triangular, trapezoidal or Gaussian) and the selection of the inference and defuzzification methods can be configured by those skilled in the art according to actual needs. These are all well-known technologies in the art and will not be described in detail here.

[0102] The state estimation module 50 is designed to integrate multi-source heterogeneous data from UWB distance measurements and robot body sensors to perform real-time and accurate estimation of the motion state of the tracked robot relative to the target to be followed in a state estimation filter. In one embodiment of the present invention, the state estimation filter is implemented using an extended Kalman filter (EKF), and its standard workflow is as follows.

[0103] First, define the motion state vector of the tracked robot relative to the target to be followed. In one embodiment, the state vector can be defined as: ;in, and This represents the relative position components of the tracked robot in a two-dimensional coordinate system with the target to be followed as the origin. and This represents the relative velocity component of the tracked robot with respect to this coordinate system.

[0104] The extended Kalman filter operates in a prediction-update loop.

[0105] S251, Perform state prediction; this step uses robot body sensor data to predict the current state based on the state estimate from the previous moment; the robot body sensor data includes robot motion information acquired by the inertial measurement unit and wheel speed encoder, which is integrated into a control input vector. The state prediction process is described by the following two formulas:

[0106] ;

[0107] ;

[0108] in: yes The final state estimate at time t; Based on Time-state pair Predicted value of the state at any given time; It is a nonlinear state transition function established based on the robot's kinematic model; yes The state covariance matrix at time t; yes The predicted state covariance matrix at time 1; It is a state transition function exist The Jacobian matrix calculated at point; It is the process noise covariance matrix, used to describe the inherent uncertainty of the motion model.

[0109] S252, Perform a state update; this step utilizes the UWB distance measurement value obtained from the UWB receiver at the current time. The predicted state obtained in S251 is corrected; the state update process specifically includes: calculating the Kalman gain. : ;

[0110] Update state estimate : ;

[0111] Update the state covariance matrix : ;

[0112] in: This is the UWB distance measurement value at the current moment; It is the observation function, which takes the state vector The predicted location component is converted into a predicted distance value, i.e. ; It is the observation function exist The Jacobian matrix calculated at point; It is the noise covariance, which characterizes the degree of uncertainty in UWB distance measurements; It is an identity matrix.

[0113] The result obtained after the state update step and This is the final output of the robot's motion state estimate at the current moment and its corresponding uncertainty covariance; this state estimate will be provided to the path planning module 30 and the underlying motion controller for subsequent decision-making and control.

[0114] The state estimation module 50 measures the measurement noise covariance in its state update step. The application has been improved to dynamically reflect the real-time reliability of UWB distance measurements. The specific implementation is as follows.

[0115] S253, applying dynamic measurement noise covariance; during the state update phase of each control cycle, the state estimation module 50 receives the measurement noise covariance value output from the dynamic covariance reconstruction module 40, which precisely corresponds to the current channel state; the state estimation module 50 directly uses this received value as the current time. Measurement noise covariance And substitute it into the calculation of Kalman gain In the formula: ;

[0116] S254 enables adaptive state updates; by dynamically changing... The values ​​are applied to the above formula, and the state estimation module 50 implements adaptive weighted fusion of UWB distance measurements; the adjustment mechanism of this process is as follows:

[0117] From the formula for calculating Kalman gain, we can see that the measurement noise covariance... It is a key variable that determines the magnitude of the Kalman gain; when the channel state identification module 10 determines that the current signal propagation state is poor (for example, in a hard non-line-of-sight state), the dynamic covariance reconstruction module 40 will output a larger value. Numerical value; due to In the inverse term of the Kalman gain calculation formula, there is an addition term, a larger one. The value will lead to As the value of increases, its reciprocal decreases accordingly, ultimately leading to a change in the calculated Kalman gain. Decrease.

[0118] In the state update formula In the middle, a smaller Kalman gain It will reduce the measurement residual. The contribution to the state update; the direct effect of this process is that the system reduces the current UWB distance measurement. The level of trust depends more on the state prediction values ​​obtained from the robot's motion model in the state prediction step. .

[0119] Conversely, when the signal propagation is good, the dynamic covariance reconstruction module 40 outputs a smaller value. The value makes the Kalman gain... This increases the value, thereby increasing the UWB distance measurement. The weights in state fusion enable the state estimation results to converge more quickly to the high-confidence measurement.

[0120] In this way, the state estimation module 50 can dynamically adjust the degree of trust in the UWB distance measurement value according to the real-time propagation quality of the UWB signal, suppress its adverse effects when the signal quality is poor, and make full use of its information when the signal quality is good, thereby ensuring the stability and robustness of the positioning results of the entire autonomous following system.

[0121] See attached document Figure 3 , Figure 3 yes Figure 1 The diagram shows the state transition of the autonomous following mode controller. The autonomous following mode controller 60 is designed to globally adjust the operation strategy of the entire autonomous following system based on the reliability of UWB positioning information at the current moment, in order to ensure the safety and smoothness of the operation. The specific implementation process of this module is as follows.

[0122] S261 converts the measurement noise covariance into a quantified follow-up confidence index; the autonomous follow-up mode controller 60 receives the measurement noise covariance value, which characterizes the uncertainty of the current UWB distance measurement, and is output in real time by the dynamic covariance reconstruction module 40. ;because The numerical value is inversely proportional to the reliability of the measured value. To obtain a more intuitive evaluation index that is directly proportional to reliability, the autonomous following mode controller 60 uses a preset mapping function to... Converted into a standardized follow-up confidence index ranging from 0 to 1. In one embodiment, the mapping function can take an exponentially decaying form:

[0123] ;

[0124] in: This is a normal number, used as an adjustable scaling factor to control the scaling factor of this metric. Sensitivity to change;

[0125] This function provides a larger measurement noise covariance value. This will correspond to a low follow-through confidence index close to 0, while a smaller one... The value corresponds to a high follow-up confidence index close to 1.

[0126] S262, based on a threshold judgment of the follow confidence index, switches the system's operating mode; the autonomous follow mode controller 60 internally presets at least two confidence thresholds: a high confidence threshold... and low confidence threshold The autonomous follow mode controller 60 will use the current follow confidence index calculated in S261. The system compares the results against these two thresholds and switches between several preset operating modes based on the comparison results:

[0127] like If the system enters or remains in the "standard autonomous following" mode, the system considers the current positioning information to be highly reliable, and the tracked robot will follow the target according to the complete speed command output by the path planning module 30.

[0128] like If the system enters or switches to the "slow-down cautious following" mode, the system considers that the reliability of the current positioning information has decreased. The autonomous following mode controller 60 will send a speed limit command to the robot's underlying motion controller, for example, limiting the robot's maximum speed to a preset percentage (e.g., 50%) of its maximum speed in standard mode, so as to continue to perform the following task in a more conservative manner.

[0129] like If the system enters the "safe hovering" mode, it considers that the reliability of the current positioning information is below the acceptable safety limit. The autonomous following mode controller 60 will immediately send a braking command to the underlying motion controller to make the tracked robot decelerate smoothly and stop at the current position. At the same time, the system can trigger a human-machine interface to issue an alarm, prompting the operator that the current automatic following function has been suspended and is waiting for the positioning signal to be restored or for manual intervention.

[0130] Through the above steps, the autonomous follow mode controller 60 improves and transforms the underlying signal quality information into macroscopic system behavior decisions, and adjusts its operating strategy based on the real-time quality of the positioning data to ensure system security and mission execution success rate.

[0131] To ensure the stability and robustness of the tracked robot's autonomous following in complex agricultural power grid operation environments, this invention adopts a collaborative control strategy at the system level that combines active avoidance and passive adaptation. This strategy integrates multiple functional modules such as prediction, planning, perception, and estimation of the system, forming a closed-loop control system with dual protection.

[0132] The first layer of protection for this collaborative control strategy is implemented by the path planning module 30, which constitutes an active avoidance mechanism. This mechanism avoids potential signal quality degradation areas in advance through path planning. Specifically, in its path evaluation function, the path planning module 30 not only considers physical obstacles determined by LiDAR data, but also integrates prediction information about future signal propagation quality provided by the channel state prediction module 20. When it is predicted that a candidate driving trajectory will cause the robot to enter a signal occlusion or strong multipath region, the path planning module 30 will assign a higher signal cost value to the trajectory. By optimizing the signal cost term in the multi-objective fusion path evaluation function, the system will tend to select a driving trajectory that can maintain a good UWB signal propagation path, thereby actively and proactively avoiding the degradation of positioning data quality.

[0133] The second layer of protection for this collaborative control strategy is achieved through the collaboration of the dynamic covariance reconstruction module 40 and the state estimation module 50, forming a passive adaptation mechanism. This mechanism suppresses the impact of bad data at the algorithm level when the signal quality has already deteriorated. Specifically, when the channel state identification module 10 detects a deterioration in the current signal propagation state, the dynamic covariance reconstruction module 40 calculates a relatively large measurement noise covariance corresponding to the deteriorated state. Subsequently, in the extended Kalman filter algorithm of the state estimation module 50, this increased... The value will directly affect the Kalman gain. The reduction in Kalman gain means that in the state update step, the system reduces the trust weight of the current UWB distance measurement value, which contains a large error, and instead relies more on the state prediction value driven by the robot's own sensor data. This process suppresses the impact of bad measurement data on the final state estimation result from the algorithm level, ensuring the smoothness and stability of the positioning output.

[0134] In summary, the active avoidance mechanism and the passive adaptation mechanism complement each other; the former avoids problems by adjusting the physical path, while the latter adapts to problems through dynamic adjustments at the algorithm level when problems occur. These two mechanisms enable the entire autonomous following system to make optimal responses whether facing foreseeable signal challenges or sudden signal deterioration, thus forming a complete closed-loop control system with dual safeguards.

[0135] See attached document Figure 4 , Figure 4This is a schematic diagram illustrating an application scenario of the active avoidance strategy in one embodiment of the present invention. This embodiment describes a scenario in which a tracked robot autonomously follows a maintenance worker in a rural power distribution network environment; this scenario is used to illustrate how the present invention addresses the problem of UWB signal quality degradation caused by physical obstruction through a strategy combining active avoidance and passive adaptation.

[0136] Scene setting:

[0137] Environment: A rural power distribution network operation area consisting of open ground and a cement utility pole.

[0138] Task: The tracked robot (equipped with the system of this invention) needs to follow a maintenance personnel wearing a UWB beacon in real time.

[0139] Initial state: There is an unobstructed line-of-sight propagation path between the robot and the person. The two move on an open ground, and the robot is in "standard autonomous following" mode.

[0140] Workflow:

[0141] Phase 1: Standards Self-Following

[0142] In the initial state, there are no obstacles between the maintenance personnel and the robot.

[0143] Channel state identification module 10: Extracts the smaller root mean square delay spread from the channel impulse response acquired by the UWB receiver. and a larger power ratio Based on the classification logic, the current channel state is identified as "strong line-of-sight".

[0144] Dynamic covariance reconstruction module 40: Receives the channel feature quantity corresponding to "strong line-of-sight", and outputs a measurement noise covariance with a very small value through fuzzy inference. .

[0145] State estimation module 50: In the state update step of the extended Kalman filter, because The calculated Kalman gain is very small. The large size allows the filter to have high confidence in the current UWB distance measurements, resulting in accurate estimation of the robot's relative position of the person.

[0146] Autonomous Follow Mode Controller 60: Receives a very small value A high follow-up confidence index close to 1 was calculated. Because this indicator is above the high confidence threshold. The system remained in "standard autonomous following" mode, and the robot followed at a normal speed.

[0147] Phase Two: Actively Avoiding Signal Obstruction

[0148] The maintenance personnel's route will require them to go around the side of the concrete utility pole, while if the robot follows the shortest path, the straight path between it and the personnel will be temporarily blocked by the utility pole.

[0149] Channel State Prediction Module 20: This module performs trend analysis on the time series of historical channel characteristics to predict the future performance of the robot if it continues to travel along the current optimal path. After a certain time step, the channel characteristics will deteriorate; therefore, the module will classify the predicted channel state associated with the end point of the path as "hard non-line-of-sight".

[0150] Path planning module 30: In its trajectory evaluation using the dynamic window method, it receives the above prediction results. These prediction results correspond to the future endpoint position of the candidate trajectory.

[0151] For the candidate trajectory that would cause signal obstruction, the system calculates a very high signal cost value based on the "hard non-line-of-sight" prediction results. .

[0152] For another candidate trajectory that deflects outward and avoids signal blockage, its predicted channel state is "strong line-of-sight," therefore its signal cost value is... Lower.

[0153] In the multi-objective fusion path evaluation function, although the heading cost of the deflection trajectory is slightly higher, its total score is higher because its signal cost is much lower than that of the direct path. On the contrary, it is the highest.

[0154] System decision: The robot ultimately chose the slightly deviated path; this decision-making process reflects a proactive avoidance strategy, that is, the system avoids signal quality degradation in advance through path planning.

[0155] Phase Three: Passive Adaptation and Mode Switching

[0156] Due to the confined space, the active avoidance was not entirely successful, and the signal path was still severely blocked by the utility pole.

[0157] Channel State Identification Module 10: Upon signal obstruction, it detects drastic changes in channel characteristics in real time and identifies the current channel state as "hard non-line-of-sight"; simultaneously, the distance measurement value output by the UWB receiver... A significant positive deviation was observed.

[0158] Dynamic covariance reconstruction module 40: Upon receiving the feature quantity of "hard non-line-of-sight", it immediately calculates a large measurement noise covariance that matches this state. .

[0159] State estimation module 50: This value is very large. Substitute the value into EKF; Kalman gain The calculation result becomes very small; during state updates, the smaller This leads to incorrect UWB measurements. The weights in the final state estimation are reduced, and the filter relies more on the state predictions driven by the robot's own sensor data; this process reflects a passive adaptation strategy, that is, when the signal quality actually deteriorates, the system suppresses the influence of bad data at the algorithm level.

[0160] Autonomous Follow Mode Controller 60: Simultaneously, this module also receives a very large value. And calculate a threshold below the low confidence level. Follow-up confidence index The system immediately switches from "standard autonomous following" mode to "safe hovering" mode, applies braking and stops moving, and simultaneously sends an alarm to the host computer or remote controller indicating that the positioning signal has been lost.

[0161] Phase Four: System Recovery

[0162] The maintenance personnel walked past the utility pole, and the line-of-sight path between them and the robot was re-established.

[0163] System status recovery: Channel status restored to "strong line-of-sight". The value returned to normal levels, following the confidence index. It has rebounded to a high level.

[0164] Mode switching: When the autonomous follow mode controller detects that the confidence level has recovered, it automatically switches the system back to the "standard autonomous follow" mode.

[0165] Task continues: The robot resumes normal speed and continues to follow the maintenance personnel steadily.

[0166] This embodiment demonstrates the collaborative operation of proactive avoidance and passive adaptation strategies. The system preemptively attempts to avoid communication problems through path planning; when a problem occurs, it adjusts the state estimation algorithm to adapt to adverse data, and the top-level controller adjusts the behavior mode to ensure safety, ultimately automatically returning to normal operation after the signal is restored.

[0167] 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. An autonomous following system for tracked robots operating agricultural power distribution networks based on UWB positioning, characterized in that, include: UWB beacons associated with the target to be followed; A UWB receiver mounted on a tracked robot and a control system associated with the UWB receiver; The control system is configured to perform the following operations: acquire channel impulse response data corresponding to the signal transmitted by the UWB beacon from the UWB receiver; the control system includes: The channel state identification module is configured to extract at least one channel feature from the channel impulse response data, wherein the channel feature is selected from the root mean square delay spread, the power ratio of the first path component and the strongest path component, and to identify the current channel state characterizing the current signal propagation path state based on the channel feature. A state estimation module includes a state estimation filter, which fuses UWB distance measurements and robot body sensor data to estimate the motion state of the tracked robot relative to the target to be followed. The model parameters of the state estimation filter include the measurement noise covariance corresponding to the UWB distance measurements. The control system is configured to dynamically adjust the value of the measurement noise covariance based on the current channel state. The channel state prediction module is used to predict the future channel state based on the historical time series of channel impulse response data. The path planning module is configured to adjust the travel path of the tracked robot based on the predicted channel state; wherein, the path planning module is configured to generate a dynamic signal cost map based on the predicted channel state, the cost value in the dynamic signal cost map is inversely proportional to the predicted signal propagation quality, and the dynamic signal cost map is used as a cost item in the path planning evaluation function. The control system is configured to operate a path planning module and a state estimation module in a coordinated manner. When the predicted channel state indicates that the signal quality will deteriorate in the future, the path planning module will be used to adjust the driving path to actively avoid the degradation. When the current channel state indicates that the current signal quality has deteriorated, the model parameters of the state estimation filter will be dynamically adjusted to ensure the stability of the state estimation.

2. The UWB-based tracking-based autonomous following system for agricultural power distribution network operations according to claim 1, characterized in that, The control system further includes a dynamic covariance reconstruction module, which is configured to use fuzzy inference logic to establish a mapping relationship between the channel feature quantity and the measurement noise covariance, and to calculate the real-time value of the channel feature quantity into a continuously changing measurement noise covariance value according to the mapping relationship.

3. The UWB-based tracking-based autonomous following system for agricultural power distribution network operations according to claim 1, characterized in that, The path planning evaluation function also incorporates an obstacle avoidance cost term for evaluating the distance to physical obstacles and a heading cost term for evaluating the heading deviation from the target to be followed, in order to generate a comprehensive evaluation result.

4. The UWB-based tracking-based autonomous following system for agricultural power distribution network operations according to claim 1, characterized in that, The channel state identification module is also configured to automatically collect channel impulse response data in a stable motion state as a high-confidence line-of-sight sample when the tracked robot is in a preset stable motion state, and use the high-confidence line-of-sight sample to perform online self-calibration of the current channel state identification logic.

5. The UWB-based tracking-based autonomous following system for agricultural power distribution network operations according to claim 1, characterized in that, The control system further includes an autonomous following mode controller, which is configured to convert the dynamically adjusted measurement noise covariance into a quantified following confidence index, and automatically switch between a standard autonomous following mode, a deceleration cautious following mode, and a safe hovering mode based on the value of the following confidence index.

Images