Electromagnetic interference control method for substation inspection robot

Abstract

The application discloses a transformer substation inspection robot electromagnetic anti-interference control method, and belongs to the technical field of robot intelligent control. The method collects the current position of the robot, the electromagnetic field intensity of adjacent sampling points, the motion state and the trajectory tracking state in real time; constructs a forward motion electromagnetic gradient combining the electromagnetic intensity difference and the motion direction, fuses the basic risk term and the electromagnetic motion interaction term to obtain the electromagnetic motion coupling risk and solve the risk change rate; calculates the short-time prediction risk of the robot by comprehensively considering the coupling risk, the risk change rate and the electromagnetic enhancement trend; builds a prediction type hysteresis gate relying on the short-time prediction risk, divides the risk interval, generates a flexible anti-interference factor, continuously adjusts the related parameters of the robot, simultaneously fuses the basic trajectory tracking control quantity, the error anti-interference compensation quantity and the electromagnetic gradient reverse bias compensation to obtain the final control quantity, and realizes the adaptive electromagnetic anti-interference motion control of the inspection robot in the strong electromagnetic field scene of the transformer substation.

Description

Technical Field

[0001] This invention relates to the field of intelligent robot control technology, and more specifically to an electromagnetic interference control method for a substation inspection robot. Background Technology

[0002] Substations contain high-voltage electrical equipment such as transformers, busbars, switchgear, reactors, instrument transformers, GIS equipment, and cable terminals, and the electromagnetic environment in some areas is complex. When inspection robots approach these devices or pass through areas with strong electromagnetic interference, electromagnetic disturbances may affect their sensor measurements, positioning calculations, trajectory tracking, and heading control, leading to increased position errors, heading errors, or positioning residuals, thereby affecting the stability and safety of the inspection process.

[0003] Existing control methods for substation inspection robots mainly focus on path planning, obstacle avoidance control, speed limiting, and positioning error correction, but they do not adequately consider the coupling effect between the electromagnetic environment and the robot's motion state. Some methods only use the electromagnetic field strength at the current position to make threshold judgments, which is insufficient to reflect whether the robot will enter a region with enhanced electromagnetic field after continuing to move in the current direction of motion; some methods only reduce speed or adjust the path after the positioning error or trajectory error has increased significantly, resulting in a control response lag.

[0004] Therefore, it is necessary to propose an electromagnetic disturbance rejection control method that can comprehensively consider the spatial variation of electromagnetic field, robot motion direction, trajectory tracking error and positioning residual, so as to realize the early identification, smooth switching and continuous control adjustment of robot operation risks in strong electromagnetic environment. Summary of the Invention

[0005] In view of this, the present invention provides an electromagnetic disturbance rejection control method for substation inspection robots to solve the bottleneck problems existing in the background technology. This method constructs an electromagnetic gradient in the motion direction, electromagnetic-motion coupling risk, short-time prediction risk, predictive hysteresis gating state, and a flexible disturbance rejection factor to achieve early disturbance rejection, smooth deceleration, compensation enhancement, safety distance extension, and reverse electromagnetic gradient correction for the robot in strong electromagnetic environments, thereby improving the operational stability and safety of substation inspection robots in complex electromagnetic environments.

[0006] To achieve the above objectives, the present invention adopts the following technical solution.

[0007] This invention proposes an electromagnetic interference immunity control method for a substation inspection robot, comprising the following steps: Step 1: Real-time acquisition of electromagnetic field strength at the robot's current position, electromagnetic field strength at adjacent sampling positions, motion state, and trajectory tracking state, and normalization processing of state quantities with different dimensions; the acquired quantities of the robot include electromagnetic field strength at the current position, forward electromagnetic field strength, left electromagnetic field strength, right electromagnetic field strength, electromagnetic field change rate, robot position, velocity, heading angle, position tracking error, heading angle error, and positioning residual; Step 2: Based on the difference in electromagnetic field strength between the robot's current position and adjacent sampling positions, construct the forward electromagnetic gradient and the lateral electromagnetic gradient; based on the forward electromagnetic gradient and the lateral electromagnetic gradient, construct the estimated electromagnetic field spatial gradient for the current position; based on the robot's current motion direction, construct the unit vector of the motion direction, and then construct the electromagnetic gradient of the forward motion direction. Step 3: Construct a basic risk term by fusing electromagnetic field strength, electromagnetic field change rate, electromagnetic gradient in the positive motion direction, position tracking error, heading angle error, and positioning residual; simultaneously construct an electromagnetic motion interaction term based on position error, heading angle error, and positioning residual; and fuse the basic risk term and the electromagnetic motion interaction term to obtain the electromagnetic motion coupling risk. Step 4: Calculate the rate of change of risk based on the electromagnetic motion coupling risk at two adjacent sampling times. Then, based on the robot's current electromagnetic motion coupling risk, rate of change of risk, and electromagnetic enhancement trend in the direction of motion, calculate the robot's short-term predicted risk within the time window. Step 5: Set the predictive hysteresis gating state based on the short-term predicted risk, and divide the robot's operating state into the normal inspection zone, the flexible anti-disturbance zone, the strong anti-disturbance zone, and the safe low-speed zone; and generate the flexible anti-disturbance factor based on the relative position of the short-term predicted risk in the flexible control zone after determining the risk interval. Step Six: Continuously adjust the robot's reference speed, upper limit of angular velocity, dynamic safety distance, and anti-disturbance compensation gain according to the flexible anti-disturbance factor; generate the basic trajectory tracking control quantity under the constraints of the adjusted reference speed, upper limit of angular velocity, and dynamic safety distance; generate the error anti-disturbance compensation quantity according to the anti-disturbance compensation gain and control error vector; and fuse the basic trajectory tracking control quantity, the error anti-disturbance compensation quantity, and the electromagnetic gradient reverse bias compensation quantity to obtain the robot's final control quantity, thereby completing the robot's electromagnetic anti-disturbance control.

[0008] The above-mentioned electromagnetic interference control method for substation inspection robots involves real-time acquisition of the electromagnetic field strength at the robot's current position, the electromagnetic field strength at adjacent sampling positions, the motion state, and the trajectory tracking state in step one, and normalization processing of state quantities with different dimensions. The acquired quantities of the robot include the electromagnetic field strength at the current position, the forward electromagnetic field strength, the electromagnetic field strength on the left, the electromagnetic field strength on the right, the rate of change of the electromagnetic field, the robot's position, speed, heading angle, position tracking error, heading angle error, and positioning residual.

[0009] The electromagnetic interference immunity control method for the substation inspection robot described above, the method for constructing the forward motion reverse electromagnetic gradient in step two includes: (1) Based on the difference in electromagnetic field strength between the robot's current position and adjacent sampling positions, construct the forward electromagnetic gradient and the lateral electromagnetic gradient.

[0010]

[0011] In the formula: Forward electromagnetic gradient, For lateral electromagnetic gradient, Forward sampling distance, The lateral sampling distance is ε, which is a small constant to prevent the denominator from being zero. (2) Based on the forward electromagnetic gradient and the lateral electromagnetic gradient, construct the estimated electromagnetic field spatial gradient for the current position.

[0012] In the formula: Let be the robot's forward unit direction vector. Let be the robot's lateral unit direction vector. Estimate the spatial gradient of the electromagnetic field at the current location; (3) Based on the robot's current direction of motion, construct a unit vector for the direction of motion, and then construct the electromagnetic gradient for the positive direction of motion.

[0013] In the formula: Let the unit vector be the direction of robot motion. For robot speed;

[0014] In the formula: This is used to characterize the risk trend of a robot entering a region of enhanced electromagnetic field along its current direction of motion. When the value is large, it indicates that the robot will enter the region of enhanced electromagnetic field if it continues to move in the current direction; when... When it is close to 0, it indicates that the electromagnetic enhancement trend in the current direction of motion is not obvious.

[0015] The electromagnetic interference immunity control method for the substation inspection robot described above, the electromagnetic motion coupling risk calculation method in step three includes the following steps: (1) The electromagnetic field strength, electromagnetic field change rate, electromagnetic gradient in the positive motion direction, position tracking error, heading angle error, and positioning residual are integrated to construct the basic risk item.

[0016] In the formula: The electromagnetic field strength at the current location; The absolute value of the rate of change of the electromagnetic field; The electromagnetic gradient is in the positive direction of motion; This is the normalized value of the position tracking error; This is the normalized value of the heading angle error; To locate the normalized value of the residual; each weight satisfies: ; (2) Construct electromagnetic motion interaction terms,

[0017] In the formula: For electromagnetic motion interaction terms; , and These are the interaction weights for position error, heading angle error, and positioning residual, respectively. (3) The electromagnetic motion coupling risk is obtained by coupling the basic risk term and the electromagnetic motion interaction term.

[0018] In the formula: To mitigate the risks associated with electromagnetic motion coupling, This is the adjustment coefficient for the interaction term.

[0019] In the aforementioned electromagnetic interference immunity control method for substation inspection robots, the risk change rate mentioned in step four is used to determine whether the risk is increasing or decreasing. It is calculated based on the electromagnetic motion coupling risk at two adjacent sampling times.

[0020] In the formula: The electromagnetic-motion coupling risk at the current moment; The electromagnetic-motion coupling risk at the previous sampling time; when This indicates that the robot's environment or movement is deteriorating; when This indicates that the current risk has not continued to escalate.

[0021] The electromagnetic interference immunity control method for the substation inspection robot described above, wherein the short-time prediction risk calculation formula is as follows:

[0022] In the formula: For the prediction time window T pShort-term risk forecasting afterwards; This represents the predicted increment of risk over time; This indicates the increased risk associated with the robot entering the electromagnetic enhancement region along its current direction of motion. This represents the risk correction amount resulting from the interaction between electromagnetic disturbances and motion deviations. This is a correction factor for the movement trend; This is a correction factor for interaction risk. This is the amplitude limiting function.

[0023] In the above-mentioned electromagnetic interference control method for substation inspection robots, step five sets entry thresholds for different risk ranges. and launch threshold The entry threshold is greater than the exit threshold. This can form a hysteresis interval, preventing the robot from frequently entering and exiting the disturbance-resistant state when the risk value fluctuates slightly; When the predicted risk reaches the entry threshold and the current state holding time meets the minimum holding time, the robot enters the next higher risk zone; when the predicted risk is lower than the exit threshold and the electromagnetic gradient in the motion direction falls back, the robot exits the current risk zone.

[0024] The electromagnetic disturbance rejection control method for the substation inspection robot described above, in step five, after determining the risk range, generates a flexible disturbance rejection factor based on the relative position of the predicted risk within the flexible control range. The method includes: (1) Constructing a relative risk position,

[0025] In the formula: x represents the relative position of the predicted risk within the flexible control range; This marks the beginning of the flexible disturbance rejection boundary. For strong anti-disturbance boundary; when hour, The robot operates according to normal inspection parameters; when lie in and In between, As the predicted risk increases smoothly; when hour, The robot enters a state of strong anti-interference or safe low-speed control. (2) Generate a flexible disturbance rejection factor through the smoothing mapping function S(·).

[0026]

[0027] In the formula: It is a flexible disturbance rejection factor, with a value range of [0,1].

[0028] In the above-mentioned electromagnetic interference control method for substation inspection robots, step six is ​​based on the flexible interference immunity factor. Continuously adjust the robot's reference speed, upper limit of angular velocity, dynamic safety distance, and disturbance rejection compensation gain, specifically as follows: a. Based on the flexible disturbance rejection factor Adjust the robot's reference speed.

[0029] In the formula: The adjusted reference speed, For normal inspection speed, This is the speed adjustment coefficient. The minimum permissible speed is defined by `clip{·}`, which is the clipping function. As the flexibility immunity factor increases, the robot's reference speed gradually decreases, thereby reducing motion shock and trajectory tracking errors within the strong electromagnetic disturbance region. b. Based on the flexible interference immunity factor Adjust the robot's angular velocity limit.

[0030] In the formula: This is the adjusted upper limit of angular velocity. This is the upper limit of angular velocity during normal inspection. This is the angular velocity adjustment coefficient. This is the upper limit of the minimum angular velocity. As the risk level increases, the maximum allowable turning speed of the robot decreases to suppress the amplification of heading errors caused by rapid turning; c. Based on the flexible disturbance rejection factor Adjust the dynamic safety distance.

[0031] In the formula: For dynamic safety distance, Based on the safe distance, This is a safety distance adjustment coefficient; the dynamic safety distance is used to constrain the minimum permissible distance between the robot and electrified equipment, obstacles, or restricted areas; when the actual distance is less than... When this happens, the robot reduces its speed or switches to a safe low-speed state; d. Adjust the disturbance rejection compensation gain according to the flexible disturbance rejection factor λ(t).

[0032] In the formula: This is the current anti-disturbance compensation gain matrix. This is the basic compensation gain matrix under normal conditions. The maximum disturbance rejection compensation gain matrix is ​​used; the disturbance rejection compensation gain matrix is ​​multiplied by the control error vector to obtain the disturbance rejection compensation amount. The error disturbance rejection compensation amount has the same dimension as the robot's final control quantity. As the flexible disturbance rejection factor increases, the control system's ability to compensate for position errors, heading angle errors, and positioning residuals is enhanced, thereby improving the robot's trajectory tracking stability in strong electromagnetic environments.

[0033] The above-mentioned electromagnetic interference suppression control method for substation inspection robots is characterized in that, in step six, the adjusted reference speed, upper limit of angular velocity, and dynamic safety distance are respectively used as the speed setpoint, steering control limit, and safety distance constraint of the robot's basic trajectory tracking controller to generate basic trajectory tracking control quantities; and error interference suppression compensation quantities are generated based on the control error vector and error interference suppression compensation gain matrix. The basic trajectory tracking control quantities, error interference suppression compensation quantities, and electromagnetic gradient reverse bias compensation quantities are then fused to obtain the final control quantities of the robot. The control error vector is:

[0034] In the formula, The orientation position tracking error component is calculated using the following formula:

[0035] In the formula, The position of the matching point on the reference trajectory at the current moment. This is the robot's current position. Let be the normal unit vector of the reference trajectory at the matching point. This is the normalization threshold for position tracking error; The signed heading angle error component is calculated according to the following formula:

[0036] In the formula, For reference heading angle, This is the robot's actual heading angle. An angle processing function to limit the heading angle difference within a preset angle range. The normalized threshold for heading angle error; To locate the residual correction components, calculate according to the following formula:

[0037] In the formula, The positioning residual output by the positioning solution module. To locate the residual normalization threshold, This is the normalized positioning residual; the amplitude of the positioning residual correction component increases as the positioning residual increases, and its correction direction is determined by the directional position tracking error component. Sure; The error disturbance rejection compensation amount is:

[0038] In the formula, This is the amount of error immunity compensation. The error immunity compensation gain matrix is ​​used to map the control error vector into a compensation control quantity that is consistent with the dimension of the robot's final control quantity. The final control variable for the robot is:

[0039] In the formula, This refers to the final control quantity for the robot. The basic trajectory tracking control quantity generated by the robot's basic trajectory tracking controller under the adjusted reference velocity, upper limit of angular velocity, and dynamic safety distance constraints; The normalized electromagnetic gradient in the positive direction of motion; This is the electromagnetic gradient reverse bias compensation coefficient; It is a flexible disturbance rejection factor; The control bias direction vector is obtained by mapping the electromagnetic field spatial gradient direction to the robot motion control space through robot kinematics. The control bias direction vector has the same dimension as the robot's final control quantity. Since the electromagnetic gradient reverse bias compensation term is preceded by a negative sign, the electromagnetic gradient reverse bias compensation amount is used to reduce the robot's motion component toward the direction of electromagnetic field enhancement, thereby guiding the robot away from the electromagnetic field enhancement region.

[0040] As can be seen from the above technical solution, compared with the prior art, the method of the present invention can identify the risk increase trend in advance based on the electromagnetic gradient in the positive motion direction and short-term predicted risk, and continuously adjust the reference speed, upper limit of angular velocity, dynamic safety distance, and error immunity compensation gain matrix through a flexible anti-disturbance factor. The adjusted reference speed, upper limit of angular velocity, and dynamic safety distance are used as the speed setpoint, steering control limit, and safety distance constraint of the basic trajectory tracking controller, respectively, to generate the basic trajectory tracking control quantity; the error immunity compensation gain matrix generates the error immunity compensation quantity based on the control error vector, thereby correcting the position tracking error, heading angle error, and positioning residual; the electromagnetic gradient reverse bias compensation quantity is used to reduce the motion component of the robot towards the direction of electromagnetic field enhancement. Thus, the method of the present invention can achieve early deceleration, smooth steering, safety distance extension, error compensation enhancement, and electromagnetic gradient reverse correction, improving the operational stability and safety of substation inspection robots in complex electromagnetic environments. Attached Figure Description

[0041] Figure 1 A flowchart of the electromagnetic interference suppression control method for substation inspection robots; Figure 2 Flowchart for constructing the electromagnetic gradient in the direction of motion; Figure 3 Flowchart for electromagnetic motion coupling risk and short-term prediction; Figure 4 This is a flowchart of continuous adjustment of motion control based on flexible disturbance rejection factor. Detailed Implementation

[0042] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0043] Example 1: This example provides an electromagnetic interference suppression control method for a substation inspection robot, such as... Figure 1 As shown, the method includes the following steps: Step 1: Electromagnetic-motion state acquisition and normalization processing During substation inspections, the robot collects real-time data on the electromagnetic field strength at its current location, the electromagnetic field strength at adjacent sampling locations, its motion status, and its trajectory tracking status. The adjacent sampling locations include at least the robot's forward sampling point, left sampling point, and right sampling point.

[0044] The data collected includes: electromagnetic field strength at the current location. Forward electromagnetic field strength Electromagnetic field strength on the left side Electromagnetic field strength on the right side Electromagnetic field change rate Robot position ,speed , heading angle θ(t), position tracking error e p (t), heading angle error e θ (t) and positioning residual e r (t). Simultaneously, the position of the matching point on the reference trajectory at the current moment is obtained from the preset inspection trajectory or the robot trajectory tracking module. Reference heading angle and the normal unit vector of the reference trajectory at the matching point. The reference trajectory information is used to construct the directional position tracking error component and the signed heading angle error component required for subsequent error disturbance compensation.

[0045] To avoid the direct superposition of state quantities with different dimensions, the electromagnetic field strength, electromagnetic field rate of change, electromagnetic gradient in the direction of motion, position tracking error, heading angle error, and positioning residual are normalized:

[0046] In the formula: For variables to be normalized, For the corresponding engineering threshold, Let be the normalized variable, and sat(·) be the amplitude limiting function used to restrict the normalization result to the range [0,1]. In one implementation, the amplitude limiting function is expressed as:

[0047] For the electromagnetic field strength, electromagnetic field change rate, electromagnetic gradient in the positive motion direction, position tracking error, heading angle error, and positioning residual used in the electromagnetic motion coupling risk construction, the absolute value or magnitude of the corresponding variables is normalized to characterize the magnitude of each risk factor.

[0048] For the position tracking control error component and heading angle control error component used to generate the error immunity compensation, the sign or correction direction of the corresponding error is retained to determine the control compensation direction; the positioning residual is used to adjust the compensation intensity of the position tracking control error component.

[0049] The normalized risk state quantity and the directional control error component obtained through the above processing are used as inputs for subsequent electromagnetic motion coupling risk construction and error disturbance rejection control adjustment, respectively.

[0050] Step 2: Construct the electromagnetic gradient in the direction of motion, as follows: Figure 2 As shown, it includes: (1) Based on the difference in electromagnetic field strength between the robot's current position and adjacent sampling positions, construct the forward electromagnetic gradient and the lateral electromagnetic gradient:

[0051]

[0052] In the formula: Forward electromagnetic gradient, For lateral electromagnetic gradient, Forward sampling distance, ε is the lateral sampling distance, and ε is a small constant to prevent the denominator from being zero.

[0053] (2) Based on the aforementioned forward electromagnetic gradient and lateral electromagnetic gradient Construct the estimated electromagnetic field spatial gradient at the current location.

[0054] In the formula: Let be the robot's forward unit direction vector. Let be the robot's lateral unit direction vector. Estimate the spatial gradient of the electromagnetic field at the current location.

[0055] (3) Based on the robot's current direction of motion, construct the unit vector of motion direction:

[0056] In the formula: Let the unit vector be the direction of robot motion. For robot speed.

[0057] Further construct the electromagnetic gradient in the positive motion direction:

[0058] In the formula: This is used to characterize the risk trend of a robot entering a region of enhanced electromagnetic field along its current direction of motion. When the value is large, it indicates that the robot will enter the region of enhanced electromagnetic field if it continues to move in the current direction; when... When it is close to 0, it indicates that the electromagnetic enhancement trend in the current direction of motion is not obvious.

[0059] Step 3: Constructing the electromagnetic motion coupling risk, such as... Figure 3 As shown, it includes: The electromagnetic field strength, electromagnetic field change rate, electromagnetic gradient in the positive motion direction, position tracking error, heading angle error, and positioning residual are fused to construct the basic risk term. Let... , , , , and Let the normalized values ​​of the current position electromagnetic field strength, the absolute value of the rate of change of the electromagnetic field, the electromagnetic gradient in the positive direction of motion, the position tracking error, the heading angle error, and the positioning residual be respectively. Then the basic risk items are:

[0060] The weights satisfy the following:

[0061] To avoid the difficulty of using fixed weights to adapt to different inspection areas and electromagnetic disturbance intensities, this implementation adaptively determines the weights based on the contribution of each risk factor within the most recent sliding time window. Let the i-th normalized risk component be X. i (t), whose average contribution within the sliding time window is:

[0062] Calculate the corresponding weight based on the average contribution of each risk component:

[0063] In the formula: X i (t) correspond to the normalized values ​​of electromagnetic field strength, absolute value of electromagnetic field change rate, electromagnetic gradient in the positive motion direction, position tracking error, heading angle error and positioning residual, respectively. The average contribution of the i-th risk component within the current sliding time window; This represents the number of sampling points within the sliding time window. The sampling interval; To prevent small constants with a denominator of zero.

[0064] When a certain risk factor continues to increase within the current time window, its average contribution Increase, corresponding weight Automatically increases; when the risk factor is small or its change is not significant, its weight automatically decreases. In this way, the basic risk item... It can adaptively adjust the degree of influence of various risk factors according to the current electromagnetic environment and the robot's motion state.

[0065] To further reflect the superimposed effect between electromagnetic disturbance and motion deviation, an electromagnetic-motion interaction term is constructed:

[0066] in, It is an electromagnetic-motion interaction term. , and These are the interaction weights for position error, heading angle error, and positioning residual, respectively. When the robot moves along the electromagnetic enhancement direction, and the position error, heading angle error, or positioning residual all increase simultaneously, the system risk is further amplified through the electromagnetic-motion interaction terms.

[0067] The electromagnetic-motion coupling risk is obtained from the basic risk term and the interaction term:

[0068] In the formula: For the risk of electromagnetic-motion coupling, This is the adjustment coefficient for the interaction term.

[0069] The electromagnetic-motion coupling risk not only reflects the strength of the current electromagnetic environment, but also the comprehensive impact of electromagnetic disturbances on robot localization, trajectory tracking, and heading control, providing a basis for subsequent short-term risk prediction and flexible disturbance rejection control.

[0070] Step 4: Short-term electromagnetic disturbance risk prediction. like Figure 3 As shown, to avoid the robot entering an anti-disturbance state only after the trajectory error has significantly increased, this implementation method does not only judge based on the electromagnetic-motion coupling risk at the current moment, but also combines the risk change rate and the electromagnetic enhancement trend in the motion direction to predict the robot's position within the prediction time window T. p The level of risk afterwards.

[0071] Calculate the rate of change of risk based on the electromagnetic-motion coupling risk at two adjacent sampling times:

[0072] In the formula: The electromagnetic-motion coupling risk at the current moment; This represents the electromagnetic-motion coupling risk at the previous sampling time. The rate of change of this risk is used to determine whether the risk is increasing or decreasing. When this occurs, it indicates that the robot's environment or motion state is deteriorating; when This indicates that the current risk has not continued to escalate.

[0073] Calculate the prediction time window T p Short-term forecast risks:

[0074] In the formula: For the prediction time window T p Short-term risk forecasting afterwards; This represents the predicted increment of risk over time; This indicates the increased risk associated with the robot entering the electromagnetic enhancement region along its current direction of motion. This represents the risk correction amount resulting from the interaction between electromagnetic disturbances and motion deviations. This is a correction factor for the movement trend; This is a correction factor for interaction risk. This is the amplitude limiting function.

[0075] If the current risk is increasing, or if the robot is moving at a high speed and in the direction of increased electromagnetic field, the predicted risk may increase even if the current risk value has not yet reached the disturbance immunity threshold. In this case, the system can enter a flexible disturbance immunity state in advance, avoiding the need to switch back after the trajectory deviation, positioning residual, or heading angle error has significantly increased.

[0076] Step 5: Predictive Hysteresis Gating and Flexible Disturbance Immunity Factor Generation Predictive hysteresis gating state is set based on short-term predicted risk. The robot's operating state is divided into a normal inspection zone Z0, a flexible disturbance rejection zone Z1, a strong disturbance rejection zone Z2, and a safe low-speed zone Z3. Compared to directly switching states based on the current risk value, using predictive risk and hysteresis gating can avoid the robot frequently switching near the critical threshold, thus improving the stability of the control process.

[0077] Set entry and exit thresholds for the j-th level risk interval:

[0078] When the entry threshold is greater than the exit threshold, a hysteresis interval can be formed, preventing the robot from frequently entering and exiting the disturbance-resistant state when the risk value fluctuates slightly. When the predicted risk reaches the entry threshold and the current state holding time meets the minimum holding time, the robot enters the next higher risk interval; when the predicted risk is lower than the exit threshold and the electromagnetic gradient in the motion direction falls back, the robot exits the current risk interval.

[0079] In one implementation, the update rule for the predictive hysteresis-gated state q(t) is:

[0080] In the formula: The duration of the current state. For minimum holding time, Let be the electromagnetic gradient exit threshold for the motion direction corresponding to the j-th level of risk. When the robot enters the disturbance-resistant state, it mainly depends on whether the predicted risk increases; when the robot exits the disturbance-resistant state, it is required not only that the predicted risk decreases, but also that the electromagnetic gradient in the motion direction falls back, to avoid the robot prematurely exiting the disturbance-resistant state while still moving in the direction of electromagnetic enhancement.

[0081] After determining the risk range, a flexible disturbance rejection factor is generated based on the relative position of the predicted risk within the flexible control range. The steps include: (1) Constructing a relative risk location:

[0082] In the formula: x represents the relative position of the predicted risk within the flexible control range; This marks the beginning of the flexible disturbance rejection boundary. For strong anti-disturbance boundary; when hour, The robot operates according to normal inspection parameters; when lie in and In between, As the predicted risk increases smoothly; when hour, The robot enters a state of strong anti-interference or safe low-speed control. Then, a flexible disturbance rejection factor is generated using a smoothing mapping function:

[0083] In the formula: It is a flexible disturbance rejection factor, with a value range of [0,1].

[0084] The smoothing mapping function S(·) is defined as follows: .

[0085] Through the above processing, this step simultaneously achieves two functions: first, it utilizes predictive hysteresis gating states. First, determine the risk zone in which the robot is located to avoid frequent switching; second, utilize flexible disturbance rejection factors. By continuously adjusting subsequent control parameters to avoid sudden changes in control quantities, the robot can smoothly transition from normal inspection state to disturbance rejection control state.

[0086] Step 6: Continuous adjustment of motion control based on flexible disturbance rejection factor like Figure 4 As shown, based on the flexible disturbance rejection factor Continuously adjust the robot's reference speed, upper limit of angular velocity, dynamic safety distance, and disturbance rejection compensation gain.

[0087] a. Based on the flexible disturbance rejection factor Adjust the robot's reference speed:

[0088] In the formula: The adjusted reference speed, For normal inspection speed, This is the speed adjustment coefficient. The minimum permissible speed is clip{·}, which is the clipping function. As the flexibility disturbance rejection factor increases, the robot reference speed gradually decreases, thereby reducing motion impact and trajectory tracking error in the strong electromagnetic disturbance area.

[0089] b. Based on the flexible interference immunity factor Adjusting the robot's angular velocity limit:

[0090] In the formula: This is the adjusted upper limit of angular velocity. This is the upper limit of angular velocity during normal inspection. This is the angular velocity adjustment coefficient. This is the upper limit of the minimum angular velocity. As the level of risk increases, the maximum allowable turning speed of the robot decreases to suppress the amplification of heading error caused by rapid turning.

[0091] c. Based on the flexible disturbance rejection factor Adjusting the dynamic safety distance:

[0092] In the formula: For dynamic safety distance, Based on the safe distance, This is a safety distance adjustment coefficient; the dynamic safety distance is used to constrain the minimum permissible distance between the robot and electrified equipment, obstacles, or restricted areas; when the actual distance is less than... When necessary, the robot reduces its speed or switches to a safe low-speed state.

[0093] d. Adjust the disturbance rejection compensation gain according to the flexible disturbance rejection factor λ(t):

[0094] In the formula: For the current anti-interference compensation gain, This is the base compensation gain under normal conditions. This represents the maximum disturbance rejection compensation gain. As the flexible disturbance rejection factor increases, the control system's ability to compensate for position errors, heading angle errors, and positioning residuals is enhanced, thereby improving the robot's trajectory tracking stability in strong electromagnetic environments.

[0095] The adjusted reference speed, upper limit of angular velocity, and dynamic safety distance are used as the speed setpoint, steering control limit, and safety distance constraint of the robot's basic trajectory tracking controller, respectively, to generate the basic trajectory tracking control quantity; and the error immunity compensation quantity is generated according to the control error vector and the error immunity compensation gain matrix. The basic trajectory tracking control quantity, the error immunity compensation quantity, and the electromagnetic gradient reverse bias compensation quantity are fused to obtain the robot's final control quantity. The control error vector is:

[0096] In the formula, The orientation position tracking error component is calculated using the following formula:

[0097] In the formula, The position of the matching point on the reference trajectory at the current moment. This is the robot's current position. Let be the normal unit vector of the reference trajectory at the matching point. This is the normalization threshold for position tracking error; The signed heading angle error component is calculated according to the following formula:

[0098] In the formula, For reference heading angle, This is the robot's actual heading angle. An angle processing function to limit the heading angle difference within a preset angle range. The normalized threshold for heading angle error; To locate the residual correction components, calculate according to the following formula:

[0099] In the formula, The positioning residual output by the positioning solution module. To locate the residual normalization threshold, This is the normalized positioning residual; the amplitude of the positioning residual correction component increases as the positioning residual increases, and its correction direction is determined by the directional position tracking error component. Sure; The error disturbance rejection compensation amount is:

[0100] In the formula, This is the amount of error immunity compensation. The error immunity compensation gain matrix is ​​used to map the control error vector into a compensation control quantity that is consistent with the dimension of the robot's final control quantity. The final control variable for the robot is:

[0101] In the formula, This refers to the final control quantity for the robot. The basic trajectory tracking control quantity generated by the robot's basic trajectory tracking controller under the adjusted reference velocity, upper limit of angular velocity, and dynamic safety distance constraints; The normalized electromagnetic gradient in the positive direction of motion; This is the electromagnetic gradient reverse bias compensation coefficient; It is a flexible disturbance rejection factor; The control bias direction vector is obtained by mapping the electromagnetic field spatial gradient direction to the robot motion control space through robot kinematics. The control bias direction vector has the same dimension as the robot's final control quantity. Because the electromagnetic gradient reverse bias compensation term is preceded by a negative sign, the electromagnetic gradient reverse bias compensation amount is used to reduce the robot's motion component towards the direction of electromagnetic field enhancement, thereby guiding the robot away from the electromagnetic field enhancement region. In one feasible implementation, Construct it according to the following formula:

[0102] When q(t) = Z3, or When the preset upper limit is reached and continues to exceed the safety judgment time, the robot performs actions such as slowing down safely, pausing, repositioning, or returning to a safe path; when the predicted risk decreases and the exit conditions are met, the robot proceeds according to... The continuous changes gradually restored the normal inspection parameters.

[0103] Through the above control adjustments, the robot can smoothly decelerate, enhance its anti-interference capabilities, extend its safe distance, and move away from areas of electromagnetic enhancement in strong electromagnetic environments, thereby improving the safety and stability of the inspection process.

[0104] Example 2: An electromagnetic interference control method for a substation inspection robot disclosed in this embodiment of the invention involves setting up the inspection robot in the inspection channel near the substation GIS equipment and busbars. The robot travels along a preset inspection route and passes through areas where the electromagnetic field strength gradually increases and then decreases.

[0105] In this embodiment, the robot's normal inspection speed is set to 0.6 m / s, the sampling interval is set to 0.1 s, and the prediction time window T is set to... pSet to 1.0 s, the flexible disturbance rejection start boundary Set to 0.45, strong disturbance rejection boundary The value was set to 0.75. During the test, the normalized electromagnetic field strength at the robot's current position gradually increased from 0.28 to 0.86. The specific process of this method is as follows: Step 1: Control the robot to approach the strong electromagnetic equipment along the same inspection route, and collect data in real time on the electromagnetic field strength at the current position, the electromagnetic field strength at the forward sampling point, the electromagnetic field strength at the left sampling point, the electromagnetic field strength at the right sampling point, the robot speed, the heading angle, the position tracking error, the heading angle error, and the positioning residual.

[0106] Step 2: Normalize the collected electromagnetic field strength, electromagnetic field change rate, electromagnetic gradient in the positive motion direction, position tracking error, heading angle error, and positioning residual. Construct the electromagnetic gradient in the positive motion direction based on the difference in electromagnetic field strength between the robot's current position and adjacent sampling points.

[0107] When the normalized intensity of the electromagnetic field at the robot's current position is 0.58, although it has not yet reached the conventional strong anti-disturbance threshold, the continuous increase in the electromagnetic gradient in the positive motion direction indicates that the robot will enter the electromagnetic field enhancement region if it continues to move in the current direction.

[0108] Step 3: Based on the normalized electromagnetic field strength, electromagnetic field change rate, electromagnetic gradient in the positive motion direction, position tracking error, heading angle error, and positioning residual, construct the electromagnetic motion coupling risk, and calculate the short-term prediction risk by combining the risk change rate, robot speed, electromagnetic gradient in the positive motion direction, and electromagnetic-motion interaction term.

[0109] Test results show that when the normalized intensity of the electromagnetic field at the current location is 0.58, the short-term prediction risk has already increased to 0.76. Therefore, the method of the present invention can predict the upward trend of risk in advance when the electromagnetic field intensity at the current location has not yet reached the strong anti-interference threshold.

[0110] Step 4: Update the predictive hysteresis gating state based on the short-term predicted risk and generate a flexible disturbance rejection factor.

[0111] When the short-term predicted risk increased to 0.76, the robot entered the flexible immunity zone ahead of schedule. As the robot continued to approach the electromagnetic enhancement area, the flexible immunity factor smoothly increased from 0.18 to 0.82, indicating that the immunity control strength continuously increased with the increase of risk, rather than abruptly changing from a normal inspection state to a strong immunity state.

[0112] Step 5: Continuously adjust the robot's reference speed, upper limit of angular velocity, dynamic safety distance, and error immunity compensation gain matrix based on the flexible immunity factor. Use the adjusted reference speed, upper limit of angular velocity, and dynamic safety distance as the speed setpoint, steering control limit, and safety distance constraint of the basic trajectory tracking controller, respectively, to generate the basic trajectory tracking control quantity. Simultaneously, construct the control error vector based on the robot's current position, reference trajectory information, heading angle, and positioning residual, and generate the error immunity compensation quantity through the error immunity compensation gain matrix.

[0113] Under the method of this invention, the robot's reference speed gradually decreases from 0.6 m / s to 0.32 m / s, the upper limit of angular velocity smoothly decreases from 0.8 rad / s to 0.46 rad / s, the dynamic safety distance increases from 0.80 m to 1.05 m, and the disturbance rejection compensation gain increases simultaneously. These adjustments enable the robot to decelerate, tighten steering constraints, and expand the safety distance before entering a strong electromagnetic field, thereby reducing sudden speed changes and sudden heading corrections. Step Six: Fuse the basic trajectory tracking control quantity, the error immunity compensation quantity, and the electromagnetic gradient back bias compensation quantity to obtain the final control quantity for the robot. The basic trajectory tracking control quantity is used to satisfy the preset inspection path, reference speed, angular velocity upper limit, and dynamic safety distance constraints; the error immunity compensation quantity is used to correct the robot's motion state based on position tracking error, heading angle error, and positioning residual; the electromagnetic gradient back bias compensation quantity is used to reduce the robot's motion component towards the direction of electromagnetic field enhancement, enabling the robot to preferentially move away from local areas where the electromagnetic field continues to strengthen, while still satisfying the original inspection path constraints.

[0114] When passing through the electromagnetically enhanced region, after adopting the method of the present invention, the peak value of the robot's position tracking error is reduced from 0.31 m under conventional control to 0.18 m, the peak value of the heading angle error is reduced from 8.4° to 4.7°, the peak value of the positioning residual is reduced from 0.37 m to 0.22 m, and the minimum distance between the robot and the powered equipment is increased from 0.74 m to 0.93 m.

[0115] Step 7: After the robot leaves the electromagnetic enhancement area, the normalized intensity of the electromagnetic field at the current position gradually decreases, the electromagnetic gradient in the positive motion direction falls back, and the short-term predicted risk decreases from 0.76 to 0.38. Because this invention employs a predictive hysteresis gating mechanism, the robot does not frequently switch near the risk threshold. Instead, it gradually exits the anti-disturbance state after the predicted risk falls below the exit threshold, the electromagnetic gradient in the positive motion direction falls back, and the minimum hold time is met. Subsequently, the flexible anti-disturbance factor smoothly decreases from 0.82 to 0, and the robot's reference speed, upper limit of angular velocity, dynamic safety distance, and anti-disturbance compensation gain gradually return to normal inspection parameters.

[0116] To demonstrate the anti-interference control effect of the method of the present invention on the inspection robot in the strong electromagnetic environment of the substation, this embodiment also sets up a conventional method as a control. The conventional control method judges only based on the electromagnetic field strength at the current position. It only enters the strong anti-interference state when the normalized intensity of the electromagnetic field at the current position reaches 0.75. At this time, the robot has approached the center of the electromagnetic enhancement area.

[0117] The test results are shown in Table 1.

[0118]

[0119] As shown in Table 1, conventional control methods rely solely on the electromagnetic field strength at the current position for threshold judgment. Typically, the robot only enters a strong anti-disturbance state after approaching the center of the electromagnetic enhancement region, easily leading to problems such as control response lag, sudden changes in speed commands, sudden changes in heading correction, and increased motion errors. In contrast, the method of this invention can identify the upward trend of risk in advance based on the electromagnetic gradient in the forward motion direction and short-term predicted risk, and continuously adjust the reference speed, upper limit of angular velocity, dynamic safety distance, and anti-disturbance compensation gain through a flexible anti-disturbance factor. Simultaneously, the electromagnetic gradient reverse bias compensation can guide the robot away from the direction where the electromagnetic field continues to strengthen. Therefore, the method of this invention can reduce position tracking error, heading angle error, and positioning residual, improve the safety distance between the robot and energized equipment, and enhance the operational stability and safety of substation inspection robots in complex electromagnetic environments.

[0120] The descriptions disclosed in each embodiment of this specification enable those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A method for electromagnetic interference suppression control of a substation inspection robot, characterized in that, Includes the following steps: Step 1: Real-time acquisition of electromagnetic field strength at the robot's current location, electromagnetic field strength at adjacent sampling locations, motion state, and trajectory tracking state, and normalization processing of state variables with different dimensions; Step 2: Based on the robot's current position, the difference in electromagnetic intensity between adjacent sampling positions, and the robot's current direction of motion, construct the electromagnetic gradient in the positive direction of motion; Step 3: Construct the basic risk term and the electromagnetic motion interaction term separately, and merge the two to obtain the electromagnetic motion coupling risk; Step 4: Calculate the rate of change of risk based on the electromagnetic motion coupling risk at two adjacent sampling times. Then, based on the robot's current electromagnetic motion coupling risk, rate of change of risk, and electromagnetic enhancement trend in the direction of motion, calculate the robot's short-term predicted risk within the time window. Step 5: Set the predictive hysteresis gating state according to the short-term predicted risk, divide the robot's operating state into different risk intervals, and generate a flexible disturbance rejection factor based on the relative position of the short-term predicted risk in the flexible control interval after determining the risk interval. Step Six: Continuously adjust the robot's reference speed, upper limit of angular velocity, dynamic safety distance, and disturbance rejection compensation gain according to the flexible disturbance rejection factor. Generate a basic trajectory tracking control quantity under the constraints of the adjusted reference speed, upper limit of angular velocity, and dynamic safety distance. Generate an error disturbance rejection compensation quantity according to the disturbance rejection compensation gain and control error vector. Then, fuse the basic trajectory tracking control quantity, the error disturbance rejection compensation quantity, and the electromagnetic gradient reverse bias compensation quantity to obtain the robot's final control quantity, thereby completing the robot's electromagnetic disturbance rejection control.

2. The electromagnetic interference suppression control method for substation inspection robots according to claim 1, characterized in that, In step one, the electromagnetic field strength at the robot's current position, the electromagnetic field strength at adjacent sampling positions, the motion state, and the trajectory tracking state are collected in real time, and the state quantities of different dimensions are normalized. The collected quantities of the robot include the electromagnetic field strength at the current position, the forward electromagnetic field strength, the electromagnetic field strength on the left, the electromagnetic field strength on the right, the rate of change of the electromagnetic field, the robot's position, speed, heading angle, position tracking error, heading angle error, and positioning residual.

3. The electromagnetic interference suppression control method for substation inspection robots according to claim 1, characterized in that, The method for constructing the forward motion reverse electromagnetic gradient in step two includes: (1) Based on the difference in electromagnetic field strength between the robot's current position and adjacent sampling positions, construct the forward electromagnetic gradient and the lateral electromagnetic gradient. ； ； In the formula: Forward electromagnetic gradient, For lateral electromagnetic gradient, Forward sampling distance, The lateral sampling distance is ε, which is a small constant to prevent the denominator from being zero. (2) Based on the forward electromagnetic gradient and the lateral electromagnetic gradient, construct the estimated electromagnetic field spatial gradient for the current position. ； In the formula: Let be the robot's forward unit direction vector. Let be the robot's lateral unit direction vector. Estimate the spatial gradient of the electromagnetic field at the current location; (3) Based on the robot's current direction of motion, construct a unit vector for the direction of motion, and then construct the electromagnetic gradient for the positive direction of motion. ； In the formula: Let the unit vector be the direction of robot motion. For robot speed; ； In the formula: This is used to characterize the risk trend of a robot entering a region of enhanced electromagnetic field along its current direction of motion. When the value is large, it indicates that the robot will enter the region of enhanced electromagnetic field if it continues to move in the current direction; when... When it is close to 0, it indicates that the electromagnetic enhancement trend in the current direction of motion is not obvious.

4. The electromagnetic interference suppression control method for substation inspection robots according to claim 1, characterized in that, The electromagnetic motion coupling risk calculation method described in step three includes the following steps: (1) The electromagnetic field strength, electromagnetic field change rate, electromagnetic gradient in the positive motion direction, position tracking error, heading angle error, and positioning residual are integrated to construct the basic risk item. ； In the formula: The electromagnetic field strength at the current location; The absolute value of the rate of change of the electromagnetic field; The electromagnetic gradient is in the positive direction of motion; This is the normalized value of the position tracking error; This is the normalized value of the heading angle error; To locate the normalized value of the residual; each weight satisfies: ； (2) Construct electromagnetic motion interaction terms, ； In the formula: For electromagnetic motion interaction terms; , and These are the interaction weights for position error, heading angle error, and positioning residual, respectively. (3) The electromagnetic motion coupling risk is obtained by coupling the basic risk term and the electromagnetic motion interaction term. ； In the formula: To mitigate the risks associated with electromagnetic motion coupling, This is the adjustment coefficient for the interaction term.

5. The electromagnetic interference suppression control method for substation inspection robots according to claim 1, characterized in that, The risk change rate mentioned in step four is used to determine whether the risk is increasing or decreasing. It is calculated based on the electromagnetic motion coupling risk at two adjacent sampling times. ； In the formula: The electromagnetic-motion coupling risk at the current moment; The electromagnetic-motion coupling risk at the previous sampling time; when This indicates that the robot's environment or movement is deteriorating; when This indicates that the current risk has not continued to escalate.

6. The electromagnetic interference immunity control method for substation inspection robots according to claim 5, characterized in that, The formula for calculating short-term forecast risk is as follows: ； In the formula: For the prediction time window T p Short-term risk forecasting afterwards; This represents the predicted increment of risk over time; This indicates the increased risk associated with the robot entering the electromagnetic enhancement region along its current direction of motion. This represents the risk correction amount resulting from the interaction between electromagnetic disturbances and motion deviations. This is a correction factor for the movement trend; This is a correction factor for interaction risk. This is the amplitude limiting function.

7. The electromagnetic interference immunity control method for substation inspection robots according to claim 1, characterized in that: Step five sets entry thresholds for different risk ranges. and launch threshold The entry threshold is greater than the exit threshold. This can form a hysteresis interval, preventing the robot from frequently entering and exiting the anti-disturbance state when the risk value fluctuates slightly; When the predicted risk reaches the entry threshold and the current state holding time meets the minimum holding time, the robot enters the next higher risk zone; when the predicted risk is lower than the exit threshold and the electromagnetic gradient in the motion direction falls back, the robot exits the current risk zone.

8. The electromagnetic interference suppression control method for substation inspection robots according to claim 1, characterized in that, In step five, after determining the risk range, a flexible disturbance rejection factor is generated based on the relative position of the predicted risk within the flexible control range. The method includes: (1) Constructing a relative risk position, ； In the formula: x represents the relative position of the predicted risk within the flexible control range; This marks the beginning of the flexible disturbance rejection boundary. For strong anti-disturbance boundary; when hour, The robot operates according to normal inspection parameters; when lie in and In between, As the predicted risk increases smoothly; when hour, The robot enters a state of strong anti-interference or safe low-speed control. (2) Generate a flexible disturbance rejection factor through the smoothing mapping function S(·). ； ； In the formula: It is a flexible disturbance rejection factor, with a value range of [0,1].

9. The electromagnetic interference suppression control method for a substation inspection robot according to claim 1, characterized in that, In step six, based on the flexible disturbance rejection factor The robot's reference speed, angular velocity limit, dynamic safety distance, and disturbance rejection compensation gain are continuously adjusted, specifically as follows: a. Based on the flexible interference immunity factor Adjust the robot's reference speed. ； In the formula: The adjusted reference speed, For normal inspection speed, This is the speed adjustment coefficient. The minimum permissible speed is defined by clip{·}, which is the clipping function. As the flexibility disturbance rejection factor increases, the robot's reference speed gradually decreases, thereby reducing motion shock and trajectory tracking errors within the strong electromagnetic disturbance region. b. Based on the flexible interference immunity factor Adjust the robot's angular velocity limit. ； In the formula: This is the adjusted upper limit of angular velocity. This is the upper limit of angular velocity during normal inspection. This is the angular velocity adjustment coefficient. This is the upper limit of the minimum angular velocity; As the level of risk increases, the maximum allowable turning speed of the robot decreases in order to suppress the amplification of heading error caused by rapid turning; c. Based on the flexible disturbance rejection factor Adjust the dynamic safety distance. ； In the formula: For dynamic safety distance, Based on the safe distance, This is a safety distance adjustment coefficient; the dynamic safety distance is used to constrain the minimum permissible distance between the robot and electrified equipment, obstacles, or restricted areas; when the actual distance is less than... When this happens, the robot reduces its speed or switches to a safe low-speed state; d. Adjust the disturbance rejection compensation gain according to the flexible disturbance rejection factor λ(t). ； In the formula: This is the current anti-disturbance compensation gain matrix. This is the basic compensation gain matrix under normal conditions. The maximum disturbance rejection compensation gain matrix is ​​used; the disturbance rejection compensation gain matrix is ​​multiplied by the control error vector to obtain the disturbance rejection compensation amount. The error disturbance rejection compensation amount has the same dimension as the robot's final control quantity. As the flexible disturbance rejection factor increases, the control system's ability to compensate for position errors, heading angle errors, and positioning residuals is enhanced, thereby improving the robot's trajectory tracking stability in strong electromagnetic environments.

10. The electromagnetic interference immunity control method for a substation inspection robot according to claim 9, characterized in that, In step six, the adjusted reference speed, upper limit of angular velocity, and dynamic safety distance are used as the speed setpoint, steering control limit, and safety distance constraint of the robot's basic trajectory tracking controller, respectively, to generate the basic trajectory tracking control quantity; and the error immunity compensation quantity is generated according to the control error vector and the error immunity compensation gain matrix. The basic trajectory tracking control quantity, the error immunity compensation quantity, and the electromagnetic gradient reverse bias compensation quantity are fused to obtain the robot's final control quantity. The control error vector is: ； In the formula, The orientation position tracking error component is calculated using the following formula: ； In the formula, The position of the matching point on the reference trajectory at the current moment. This is the robot's current position. Let be the normal unit vector of the reference trajectory at the matching point. This is the normalization threshold for position tracking error; The signed heading angle error component is calculated according to the following formula: ； In the formula, For reference heading angle, This is the robot's actual heading angle. An angle processing function to limit the heading angle difference within a preset angle range. The normalized threshold for heading angle error; To locate the residual correction components, calculate according to the following formula: ； ； In the formula, The positioning residual output by the positioning solution module. To locate the residual normalization threshold, This is the normalized positioning residual; the amplitude of the positioning residual correction component increases as the positioning residual increases, and its correction direction is determined by the directional position tracking error component. Sure; The error disturbance rejection compensation amount is: ； In the formula, This is the amount of error immunity compensation. The error immunity compensation gain matrix is ​​used to map the control error vector into a compensation control quantity that is consistent with the dimension of the robot's final control quantity. The final control variable for the robot is: ； In the formula, For the robot's final control input; The basic trajectory tracking control quantity generated by the robot's basic trajectory tracking controller under the adjusted reference velocity, upper limit of angular velocity, and dynamic safety distance constraints; The normalized electromagnetic gradient in the positive direction of motion; This is the electromagnetic gradient reverse bias compensation coefficient; It is a flexible disturbance rejection factor; The control bias direction vector is obtained by mapping the electromagnetic field spatial gradient direction to the robot motion control space through robot kinematics. The control bias direction vector has the same dimension as the robot's final control quantity. Since the electromagnetic gradient reverse bias compensation term is preceded by a negative sign, the electromagnetic gradient reverse bias compensation term is used to reduce the robot's motion component toward the direction of electromagnetic field enhancement, thereby guiding the robot away from the electromagnetic field enhancement region.

Images