Trajectory tracking control method of substation inspection robot

Abstract

The invention discloses a trajectory tracking control method for a substation inspection robot, relates to the technical field of substation safety inspection robot control, and adopts a finite time multi-target tracking extended Kalman filtering method to improve the trajectory tracking control accuracy on the basis of considering the position, speed, acceleration and angular velocity state values of the substation inspection robot. When the trajectory tracking trim angle and the pose state meet the finite time, the actual trajectory of the inspection robot can rapidly and accurately track the expected trajectory. According to the method, a nonlinear function in a state equation of the substation inspection robot can be taken as one item after Taylor series expansion as an initial state function of multi-target tracking, so that the problems of multiple influence factors and mutual interference of parameter sensitivities in a prediction process are solved, the anti-interference capability of the system is improved, quality improvement and efficiency improvement of power grid work are effectively promoted, and the power grid inspection robot tracking efficiency is improved. Personal and power grid safety is guaranteed.

Description

Technical Field

[0001] The invention relates to the technical field of substation safety inspection robot control, in particular to a trajectory tracking control method of a substation inspection robot. Background Art

[0002] In recent years, substation inspection robots have been increasingly used in the field of intelligent inspection. The complex working environment in substations has put forward higher requirements on the precise positioning capabilities of inspection robots. If the inspection trajectory cannot be accurately tracked, there is a risk of collision with electrical equipment, making it difficult for the robot to effectively adjust the actual inspection route. At present, the most commonly used trajectory tracking control methods for substation inspection robots are vector field histogram algorithm to adjust the robot's moving direction, preset performance backstepping anti-disturbance control, substation inspection robot positioning and mapping technology based on Gaussian projection, and improved gray wolf algorithm control method.

[0003] After searching relevant patents and literature at home and abroad, regarding the robot's milling motion control method for thin-walled parts, there are papers such as "Research on Path Planning of Substation Inspection Robot Based on Generalized Reinforcement Learning", which proposes a state-generalized deep reinforcement learning method for the path planning problem of the substation inspection robot. This method fails to achieve accurate prediction of multi-order modal parameters related to posture in the workspace. In addition, the patent application with application number 202411173919.0 and name "A Self-propelled Substation Inspection Robot" discloses a self-propelled substation inspection robot. The present invention can freely switch between leg movement and roller movement, so that the inspection robot can move normally in flat or complex road conditions indoors and outdoors for inspection work. It can not only quickly inspect the entire exterior of the substation, but also conduct close-range inspections of the interior of the substation equipment. However, this method fails to achieve coordinated vibration reduction during the inspection process. For example, the invention patent with application number 202010262991.6 and name "A detection device for the obstacle avoidance ability of a substation inspection robot" discloses a detection device for the obstacle avoidance ability of a substation inspection robot. According to the environment of the substation, at least two detectors are used to detect the position of the inspection robot, thereby improving the detection accuracy of the obstacle avoidance ability of the inspection robot, which is beneficial to improving the obstacle avoidance function of the inspection robot. This method increases the difficulty of detection because it has high requirements on the accuracy of the mathematical model.

[0004] Although there are many studies on the safety inspection methods of robots in substations in the prior art, there are few studies on the trajectory tracking control methods of inspection robots, and the problems of many factors affecting the prediction process and mutual interference of parameter sensitivities have not been solved. Summary of the invention

[0005] The technical problem to be solved by the present invention is to overcome the deficiencies of the prior art and provide a trajectory tracking control method for a substation inspection robot. This method solves the problem of multiple factors affecting the prediction process and mutual interference of parameter sensitivity, improves the anti-interference ability of the system, effectively promotes the quality and efficiency of power grid work, and ensures personal and power grid safety. At the same time, this method eliminates the problem of difficulty in establishing a control model caused by dynamic changes in load during the operation of the substation inspection robot control system, which will make the solution result more optimized, so that the global optimal solution in the application sense can be accurately obtained.

[0006] The present invention adopts the following technical solutions to solve the above technical problems:

[0007] In a first aspect, a trajectory tracking control method of a substation inspection robot proposed in the present invention comprises:

[0008] Taking the substation positioning as the inertial coordinate system, the plane motion control model of the inspection robot is established to obtain the state value and observation value of the inspection robot;

[0009] The trajectory tracking error of the substation inspection robot is obtained based on the state value and observation value of the inspection robot. The pitch angle is used as the input variable, and the virtual control input is designed in combination with the multi-target tracking extended Kalman filter state prediction.

[0010] Track the virtual control input in the inertial coordinate system and combine it with the tracking error of the posture state to complete the state update of the inspection robot;

[0011] The state values ​​of the feedforward signal and the nonlinear feedback signal are obtained by combining the position, velocity, acceleration and angular velocity provided by the inspection robot after the state update, and the extended Kalman filter controller for continuous finite-time multi-target tracking is designed.

[0012] The extended Kalman filter controller for continuous finite-time multi-target tracking takes into account the position, velocity, acceleration, and angular velocity state values. When the pitch angle and posture state of the trajectory tracking meet the finite time, the actual trajectory of the inspection robot tracks the expected trajectory.

[0013] As a further optimization scheme of the trajectory tracking control method of a substation inspection robot described in the present invention, the inspection robot establishes a planar motion control model with the substation positioning as the inertial coordinate system, and defines the state value and observation value of the inspection robot at time k, specifically:

[0014]

[0015] Among them, x k represents the position, velocity, acceleration, and angular velocity state values ​​of the inspection robot at time k, x k-1represents the position, velocity, acceleration, and angular velocity state values ​​of the inspection robot at time k-1, y k represents the observed value at time k, f(·) and h(·) represent the nonlinear function of the state value and the observed value of the inspection robot, respectively, and w k is the white noise of the inspection process at time k, v k is the interference value of the inspection process at time k, v k-1 represents the interference value of the inspection process at time k-1, n k A recursive function representing a state value.

[0016] As a further optimization scheme of the trajectory tracking control method of the substation inspection robot described in the present invention, the trajectory tracking error ε of the substation inspection robot is defined as:

[0017] ε=χ(x k )-J(η d ) (2)

[0018] Among them, χ(x k ) is the inspection robot about x k The state vector of d ) is the posture state vector including the time-varying expected trajectory of the inspection robot.

[0019] As a further optimization scheme of the trajectory tracking control method of a substation inspection robot described in the present invention, the pitch angle θ is used as an input variable, combined with multi-target tracking extended Kalman filter state prediction, and a virtual control input is designed.

[0020]

[0021] Among them, α, β and λ are adjustment parameters, diag(·) represents the trajectory tracking error diagonal matrix, sign(·) represents the trajectory tracking error sign function, represents the estimated state value of the inspection robot predicted from time k to k-1, Q k-1 represents the white noise variance of the inspection process at time k-1, is an estimate of the pitch angle θ, is the derivative of the desired trajectory.

[0022] As a further optimization scheme of the trajectory tracking control method of a substation inspection robot described in the present invention, the state update of the inspection robot is completed in combination with the tracking error of the posture state ψ, as follows:

[0023]

[0024] Among them, F k-1 The state transfer matrix representing the lateral swing position and vertical swing position of the inspection robot, Indicates F k-1 The transposed matrix, P k∣k-1 represents the prediction state error variance matrix from k to k-1, Represents x k The estimated value of represents the estimated value of the predicted posture state ψ from time k to k-1, S k represents the control matrix.

[0025] As a further optimization scheme of the trajectory tracking control method of a substation inspection robot described in the present invention, a continuous finite-time multi-target tracking extended Kalman filter controller τ is designed, which is as follows:

[0026]

[0027] Among them, γ is a Gaussian random variable, H is the inertial mass matrix, is the estimated value of the lumped interference, η is the actual trajectory of the inspection robot, and ξ is the power attenuation coefficient.

[0028] As a further optimization scheme of the trajectory tracking control method of a substation inspection robot described in the present invention, when the trajectory tracking pitch angle θ and the posture state ψ meet the finite time T, the actual trajectory η of the inspection robot tracks the expected trajectory η d ; The details are as follows:

[0029]

[0030] Among them, T h (·) is the stable time function, v, denote the second-order uniform speed and third-order constant acceleration variables, respectively. is the track friction, Represents the estimated value of the pose state ψ.

[0031] In a second aspect, an embodiment of the present invention further provides a computer device, comprising a memory, a processor, and a computer program stored in the memory and capable of running on the processor, wherein when the processor executes the computer program, the steps of the trajectory tracking control method of the substation inspection robot are implemented as described in the first aspect above or any corresponding embodiment thereof.

[0032] In a third aspect, an embodiment of the present invention further provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the trajectory tracking control method of a substation inspection robot as described in the first aspect above or any corresponding embodiment thereof are implemented.

[0033] Compared with the prior art, the present invention adopts the above technical solution and has the following technical effects:

[0034] (1) Compared with the traditional Kalman filtering method, the finite-time multi-target tracking extended Kalman filtering method based on the present invention eliminates the problem of difficulty in establishing a control model caused by dynamic load changes during the operation of the substation inspection robot control system, and will make the solution result more optimized, so that the global optimal solution in the application sense can be accurately obtained.

[0035] (2) Compared with the traditional disturbance observation method, the finite-time multi-target tracking extended Kalman filter method adopted in this method can use the nonlinear function in the state equation of the substation inspection robot as an item after Taylor series expansion as the initial state function of multi-target tracking, which solves the problem of many factors affecting the prediction process and mutual interference of parameter sensitivity, improves the system's anti-interference ability, effectively promotes the improvement of the quality and efficiency of power grid work, and ensures the safety of people and power grid. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 It is a flow chart of the present invention;

[0037] Figure 2 This is the trajectory tracking effect diagram. DETAILED DESCRIPTION

[0038] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.

[0039] like Figure 1 As shown, a trajectory tracking control method for a substation inspection robot implemented by a finite time multi-target tracking extended Kalman filter method includes the following steps:

[0040] Taking the substation positioning as the inertial coordinate system, the plane motion control model of the inspection robot is established to obtain the state value and observation value of the inspection robot; the trajectory tracking error of the substation inspection robot is obtained according to the state value and observation value of the inspection robot, and the pitch angle is used as the input variable. Combined with the multi-target tracking extended Kalman filter state prediction, the virtual control input is designed; the virtual control input is tracked in the inertial coordinate system, and the state update of the inspection robot is completed in combination with the tracking error of the posture state; in order to track the virtual control input in the inertial coordinate system, the state update of the inspection robot is completed in combination with the tracking error of the posture state, and the control input is achieved. The purpose of the chatter is to quickly eliminate the influence of the lumped interference on the multi-dimensional trajectory tracking control accuracy of the inspection robot while ensuring the continuity of the virtual control input. The state values ​​of the feedforward signal and the nonlinear feedback signal are obtained by combining the position, velocity, acceleration, and angular velocity provided by the inspection robot after the state update, and a continuous finite time multi-target tracking extended Kalman filter controller is designed; the continuous finite time multi-target tracking extended Kalman filter controller considers the position, velocity, acceleration, and angular velocity state values. When the pitch angle and posture state of the trajectory tracking meet the finite time, the actual trajectory of the inspection robot tracks the expected trajectory.

[0041] Considering that the substation inspection robot has rail friction, motion start and stop and other behaviors during the inspection process, it needs to be set as a nonlinear system. The inspection robot uses the substation positioning as the inertial coordinate system to establish a planar motion control model, and defines the state value and observation value of the inspection robot at time K as follows:

[0042]

[0043] Among them, x k represents the position, velocity, acceleration, and angular velocity state values ​​of the inspection robot at time k, x k-1 represents the position, velocity, acceleration, and angular velocity state values ​​of the inspection robot at time k-1, y k represents the observed value at time k, f(·) and h(·) represent the nonlinear function of the state value and the observed value of the inspection robot, respectively, and w k is the white noise of the inspection process at time k, v k is the interference value of the inspection process at time k, v k-1 represents the interference value of the inspection process at time k-1, n k A recursive function representing a state value.

[0044] Define the trajectory tracking error ε of the substation inspection robot,

[0045] ε=χ(x k )-J(η d ) (2)

[0046] Among them, χ(xk ) is the inspection robot about x k The state vector of d ) is the posture state vector including the time-varying expected trajectory of the inspection robot.

[0047] Taking the pitch angle θ as the input variable, combined with the multi-target tracking extended Kalman filter state prediction, the virtual control input is designed.

[0048]

[0049] Among them, α, β and λ are adjustment parameters, diag(·) represents the trajectory tracking error diagonal matrix, sign(·) represents the trajectory tracking error sign function, represents the estimated state value of the inspection robot predicted from time k to k-1, Q k-1 represents the white noise variance of the inspection process at time k-1, is an estimate of the pitch angle θ, is the derivative of the desired trajectory.

[0050] In order to track the virtual control input in the inertial coordinate system, the state update of the inspection robot is completed by combining the tracking error of the posture state ψ.

[0051]

[0052] Among them, F k-1 The state transfer matrix representing the lateral swing position and vertical swing position of the inspection robot, Indicates F k-1 The transposed matrix, P k∣k-1 represents the prediction state error variance matrix from k to k-1, Represents x k The estimated value of represents the estimated value of the predicted posture state ψ from time k to k-1, S k represents the control matrix.

[0053] In order to control input chatter, the influence of lumped disturbance on the multi-dimensional trajectory tracking control accuracy of the inspection robot can be quickly eliminated while ensuring the continuity of virtual control input. Combining the state values ​​of the position, velocity, acceleration, angular velocity precise feedforward signals and nonlinear feedback signals provided by the inspection robot, a continuous finite-time multi-target tracking extended Kalman filter controller τ is designed.

[0054]

[0055] Among them, γ is a Gaussian random variable, H is the inertial mass matrix, is the estimated value of the lumped disturbance, η is the actual trajectory of the inspection robot, and ξ is the power attenuation coefficient. In order to obtain excellent control performance, an algorithm for tracking the inspection robot in an inertial coordinate system is constructed. The designed controller τ takes into account the position, velocity, acceleration, and angular velocity state values. When the trajectory tracking pitch angle θ and the posture state ψ satisfy the finite time T of the following formula, the actual trajectory η of the inspection robot can quickly and accurately track the expected trajectory η d .

[0056]

[0057] Among them, T h (·) is the stable time function, χ, denote the second-order uniform speed and third-order constant acceleration variables, respectively. is the track friction, Represents the estimated value of the pose state ψ.

[0058] Table 1 is the control parameters of the method of the present invention. The finite time multi-target tracking extended Kalman filter method adopted by the method considers the position, velocity, acceleration, and angular velocity state values ​​of the substation inspection robot. When the trajectory tracking pitch angle and posture state meet the finite time, the actual trajectory of the inspection robot can quickly and accurately track the expected trajectory. This method can use the nonlinear function in the state equation of the substation inspection robot as an item after Taylor series expansion as the initial state function of multi-target tracking, solve the problem of many factors affecting the prediction process and mutual interference of parameter sensitivity, improve the anti-interference ability of the system, effectively promote the quality and efficiency of power grid work, and ensure the safety of people and power grid. At the same time, this method eliminates the problem of difficulty in establishing the control model caused by the dynamic change of load during the operation of the substation inspection robot control system, which will make the solution more optimized, so that the global optimal solution in the application sense can be accurately obtained. A trajectory tracking control method for a substation inspection robot implemented by a finite time multi-target tracking extended Kalman filter method can quickly deal with the parameter uncertainty problem of the inspection robot control system, thereby promoting the quality and efficiency of power grid work and ensuring the safety of people and power grid.

[0059] Table 1 is the control parameters of the method of the present invention

[0060]

[0061] The trajectory tracking effect is as follows Figure 2 As shown in Figure 2, under the action of the extended Kalman filter control method for finite-time multi-target tracking, the substation inspection robot can accurately track the desired trajectory by adjusting the pitch angle and posture state.

[0062] An embodiment of the present invention also provides a computer device, comprising a memory, a processor, and a computer program stored in the memory and capable of running on the processor, wherein when the processor executes the computer program, the steps of the trajectory tracking control method of the substation inspection robot are implemented as described in the first aspect above or any corresponding embodiment thereof.

[0063] An embodiment of the present invention also provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, it implements the steps of the trajectory tracking control method of the substation inspection robot as described in the first aspect or any corresponding embodiment thereof.

[0064] It will be appreciated by those skilled in the art that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program codes. The schemes in the embodiments of the present invention may be implemented in various computer languages, for example, object-oriented programming language Java and literal scripting language JavaScript, etc.

[0065] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0066] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0067] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process in the computer or other programmable device. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0068] Although the preferred embodiments of the present invention have been described, those skilled in the art may make other changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention.

[0069] Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.

Claims

1. A trajectory tracking control method for a substation inspection robot, characterized in that: include: Taking the substation positioning as the inertial coordinate system, the plane motion control model of the inspection robot is established to obtain the state value and observation value of the inspection robot; The trajectory tracking error of the substation inspection robot is obtained based on the state value and observation value of the inspection robot. The pitch angle is used as the input variable, and the virtual control input is designed in combination with the multi-target tracking extended Kalman filter state prediction. Track the virtual control input in the inertial coordinate system and combine it with the tracking error of the posture state to complete the state update of the inspection robot; The state values ​​of the feedforward signal and the nonlinear feedback signal are obtained by combining the position, velocity, acceleration and angular velocity provided by the inspection robot after the state update, and the extended Kalman filter controller for continuous finite-time multi-target tracking is designed. The extended Kalman filter controller for continuous finite-time multi-target tracking takes into account the position, velocity, acceleration, and angular velocity state values. When the pitch angle and posture state of the trajectory tracking meet the finite time, the actual trajectory of the inspection robot tracks the expected trajectory.

2. The trajectory tracking control method of a substation inspection robot according to claim 1 is characterized in that: The inspection robot establishes a planar motion control model with the substation positioning as the inertial coordinate system, and defines the state value and observation value of the inspection robot at time k, specifically: Among them, x k represents the position, velocity, acceleration, and angular velocity state values ​​of the inspection robot at time k, x k-1 represents the position, velocity, acceleration, and angular velocity state values ​​of the inspection robot at time k-1, y k represents the observed value at time k, f(·) and h(·) represent the nonlinear function of the state value and the observed value of the inspection robot, respectively, and w k is the white noise of the inspection process at time k, v k is the interference value of the inspection process at time k, v k-1 represents the interference value of the inspection process at time k-1, n k A recursive function representing a state value.

3. The trajectory tracking control method of a substation inspection robot according to claim 2 is characterized in that: Define the trajectory tracking error ε of the substation inspection robot, e=x(x k )-J(h d ) (2) Among them, χ(x k ) is the inspection robot about x k The state vector of d ) is the posture state vector including the time-varying expected trajectory of the inspection robot.

4. The trajectory tracking control method of a substation inspection robot according to claim 3 is characterized in that: Taking the pitch angle θ as the input variable, combined with the multi-target tracking extended Kalman filter state prediction, the virtual control input is designed. Among them, α, β and λ are adjustment parameters, diag(·) represents the trajectory tracking error diagonal matrix, sign(·) represents the trajectory tracking error sign function, represents the estimated state value of the inspection robot predicted from time k to k-1, Q k-1 represents the white noise variance of the inspection process at time k-1, is an estimate of the pitch angle θ, is the derivative of the desired trajectory.

5. The trajectory tracking control method of a substation inspection robot according to claim 4 is characterized in that: Combined with the tracking error of the posture state ψ, the state update of the inspection robot is completed as follows: Among them, F k-1 The state transfer matrix representing the lateral swing position and vertical swing position of the inspection robot, Indicates F k-1 The transposed matrix, P k∣k-1 represents the prediction state error variance matrix from k to k-1, Represents x k The estimated value of represents the estimated value of the predicted posture state ψ from time k to k-1, S k represents the control matrix.

6. The trajectory tracking control method of a substation inspection robot according to claim 5, characterized in that: Design the extended Kalman filter controller τ for continuous finite-time multi-target tracking as follows: Among them, γ is a Gaussian random variable, H is the inertial mass matrix, is the estimated value of the lumped interference, η is the actual trajectory of the inspection robot, and ξ is the power attenuation coefficient.

7. The trajectory tracking control method of a substation inspection robot according to claim 5, characterized in that: When the trajectory tracking pitch angle θ and posture state ψ satisfy the finite time T, the actual trajectory η of the inspection robot tracks the expected trajectory η d ; The details are as follows: Among them, T h (·) is the stable time function, v, denote the second-order uniform speed and third-order constant acceleration variables, respectively. is the track friction, Represents the estimated value of the pose state ψ.

8. A computer device comprising a memory, a processor, and a computer program stored in the memory and capable of running on the processor, characterized in that: When the processor executes the computer program, the steps of the trajectory tracking control method of the substation inspection robot according to any one of claims 1 to 7 are implemented.

9. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the trajectory tracking control method of the substation inspection robot as described in any one of claims 1 to 7 are implemented.

Images