Live-line robot load gravity compensation method, device and storage medium

Abstract

The application relates to the field of robot control and discloses a live working robot load gravity compensation method, equipment and a storage medium, the method comprises the following steps: constructing a gravity compensation model according to the position relationship of each joint of a robot system; designing an installation angle optimization model with error sum of squares as a constraint index based on the installation angle of a force sensor; collecting multiple sets of mechanical arm pose and force data under experimental conditions; solving the installation angle optimization model by using the multiple sets of mechanical arm pose and force data under the experimental conditions based on an improved PSO method to obtain the optimal installation angle of the force sensor; installing the force sensor through the optimal installation angle, and performing gravity compensation on the mechanical arm end through the gravity compensation model under actual working conditions to obtain accurate external environment contact force. The application has the beneficial effects that all gravity compensation parameters can be calculated at high speed and effectively, and higher-precision and higher-efficiency gravity compensation is realized.

Description

Technical Field

[0001] This invention relates to the field of robot control, and in particular to a method, device and storage medium for compensating for the load gravity of a live-line working robot. Background Technology

[0002] With the development of robot control and force sensing technologies, live-line working robots for power distribution networks are replacing manual labor in completing uninterrupted power supply operations. However, the environment for live-line working in power distribution networks is complex and diverse, and many tasks involve contact. To ensure the safe and smooth operation of the robot, there is an urgent need for it to sense external forces while interacting with the environment. This external force is not simply read directly from the force feedback of a six-dimensional force sensor, because the force or torque actually fed back by the six-dimensional force sensor mainly consists of three parts:

[0003] 1) The zero-point error of the sensor itself; 2) The gravity of the sensor tip and the additional load; 3) The force applied by the external environment. External force sensing refers to the force applied by the external environment. Therefore, load gravity compensation refers to compensating for the zero-point error of the sensor itself and the gravity of the sensor tip and the additional load.

[0004] In current gravity compensation algorithms for six-dimensional force sensors on live-line working robots in power distribution networks, the mainstream approach is to install the six-dimensional force sensor through mechanical positioning on the robot flange, assuming that the coordinate system of the end flange is aligned with the coordinate system of the sensor. However, this places very high demands on the manufacturing precision of the connectors, and there are always slight errors in mechanical positioning. Furthermore, common gravity compensation algorithms do not consider the error of the force sensor installation angle. In recent years, with the rapid development of artificial intelligence technology, a solution has been provided for the problem of calculating the force sensor installation angle, which cannot be directly displayed. An intelligent optimization algorithm can be used for approximate approximation.

[0005] Existing robot end-effector gravity compensation technologies can be divided into two types: analytical methods and machine learning.

[0006] The analytical method for gravity compensation at the end of a robot analyzes the physical relationship between various factors of force sensor perception and derives a mathematical model between the perceived force and various known quantities. However, there is an error in the installation angle of the force sensor on the robot, which is often ignored in mainstream practices, resulting in a significant error in gravity compensation accuracy.

[0007] The robot end effector gravity compensation method based on robot learning obtains the optimal gravity compensation model by collecting data samples and iteratively optimizing. However, it suffers from high training costs and high dependence. Moreover, there are various end effectors for live-line working robots in power distribution networks, which need to be converted, making this method less applicable.

[0008] In summary, existing traditional methods are prone to getting trapped in local optima, leading to solution failures. Summary of the Invention

[0009] The purpose of this invention is to address the problem that traditional analytical methods for compensating the end effector load of live-line working robots for gravity, which are prone to getting stuck in local conditions and failing to solve, due to installation angle errors. This invention proposes a method, device, and storage medium for compensating the load gravity of live-line working robots. The method includes the following steps:

[0010] S1. Construct a gravity compensation model based on the positional relationships of each joint in the robot system;

[0011] S2. Based on the installation deflection angle of the force sensor, design an angle optimization model with the sum of squared errors as the constraint index;

[0012] S3. Under experimental conditions, collect multiple sets of robotic arm pose and force data;

[0013] S4. Based on the improved PSO method, using multiple sets of robotic arm pose and force data under experimental conditions, solve the deflection angle optimization model to obtain the optimal installation deflection angle of the force sensor.

[0014] S5. By installing the force sensor at the optimal installation angle and using the gravity compensation model to perform gravity compensation on the end of the robotic arm under actual working conditions, the accurate contact force of the external environment can be obtained.

[0015] A storage medium storing instructions and data for implementing a load gravity compensation method for a live-line working robot.

[0016] A load gravity compensation device for a live-line working robot includes: a processor and a storage medium; the processor loads and executes instructions and data in the storage medium to implement a load gravity compensation method for a live-line working robot.

[0017] The beneficial effects provided by this invention are as follows: A gravity compensation model is constructed based on the positional relationships of each joint in the robot system; an angle optimization model is designed based on the installation angle of the force sensor, with the sum of squared errors as the constraint index; under experimental conditions, multiple sets of robot arm pose and force data are collected; based on the improved PSO method, the angle optimization model is solved using the multiple sets of robot arm pose and force data under experimental conditions to obtain the optimal installation angle of the force sensor; the force sensor is installed at the optimal installation angle, and under actual working conditions, the gravity compensation model is used to perform gravity compensation on the end effector of the robot arm to obtain accurate external environmental contact force. Compared with traditional gravity compensation methods based on analytical methods, this method has a better load gravity compensation effect because it uses the PSO optimization method to solve for the installation angle of the force sensor; compared with traditional gravity compensation methods based on machine learning, this method can calculate all gravity compensation parameters quickly and efficiently by using a linear decreasing adjustment model and introducing a suitable contraction factor to improve the PSO algorithm, achieving higher accuracy and efficiency in gravity compensation. Attached Figure Description

[0018] Figure 1 This is a flowchart of the method of the present invention;

[0019] Figure 2 This is a schematic diagram of the robot system layout;

[0020] Figure 3 This is a schematic diagram of the coordinate system of a live-line working robot system for power distribution networks;

[0021] Figure 4 This is an exploded view of the load gravity reference in the force sensor coordinate system;

[0022] Figure 5 This is a schematic diagram of the improved PSO algorithm for solving the optimal value of the deflection angle;

[0023] Figure 6 This is a schematic diagram comparing gravity compensation errors;

[0024] Figure 7 This is a schematic diagram of the hardware device of the present invention. Detailed Implementation

[0025] To make the objectives, technical solutions, and advantages of the present invention clearer, the embodiments of the present invention will be further described below with reference to the accompanying drawings.

[0026] Please refer to Figure 1 , Figure 1 This is a simplified flowchart of the method of the present invention;

[0027] This invention provides a method for compensating for the load gravity of a live-line working robot, comprising the following steps:

[0028] S1. Construct a gravity compensation model based on the positional relationships of each joint in the robot system;

[0029] It should be noted that, please refer to Figure 2 , Figure 2 This is a schematic diagram of the robot system layout;

[0030] The robot system consists of dual robotic arms, a six-dimensional force sensor, an end effector, and an internal computer, forming the overall gravity compensation system of the robot platform. The robotic arms are mounted on the platform surface, and the force sensor is installed between the end effector indicator and the end effector of the robotic arm. They communicate with the internal computer via data cables. When the load gravity compensation system is working normally, it collects the end effector pose data of the robotic arm and the force / torque data of the six-dimensional force sensor in real time, inputs them into the computer via the Modbus protocol, completes the real-time gravity compensation of the end effector, and obtains the real-time external contact force.

[0031] In step S1, the construction process of the gravity compensation model is as follows:

[0032] S11. Obtain the zero-point data F0 = (F... x0 F y0 F z0 T x0 T y0 T z0 The force and torque generated by the gravity of the end tool G = (G x G y G z T gx T gy T gz ), the force and torque F exerted by the external environment EX =(F EXx F EXy F EXz T EXx T EXy T EXz );

[0033] S12. Based on the parameters in step S11, obtain the force applied by the force sensor to the external environment in the coordinate system:

[0034] F EX =F-F0-G

[0035] Where F = (F x F y F z T x T y T z ), which represents the force and torque measured by a six-dimensional force sensor.

[0036] As one example, the solution for the forces applied by the external environment is as follows:

[0037] 1) Establishment of the robot system coordinate system, with the world coordinate system being O. w -X w Y w Z w The robot base coordinate system is O b -X b Y b Z b The robot end flange coordinate system is O e -X e Y e Z e The force sensor coordinate system is O. s -X s Y s Z s The relationship between them is as follows Figure 3 As shown.

[0038] Assume that the Zw direction in the world coordinate system is parallel to and opposite to the direction of gravity. And define O... s Compared to O e The attitude transformation matrix is O e Compared to O b The attitude transformation matrix is O e Compared to O w The attitude transformation matrix is

[0039] 2) Establish a model of the relationship between the force sensor and the load at the end of the robotic arm. Assuming no external force, the load gravity is placed in the force sensor coordinate system O. s Lower datum decomposition as follows Figure 4 As shown, according to the torque theorem, we can obtain:

[0040]

[0041]

[0042] By combining formulas (2) and (3), we can derive formula (4):

[0043]

[0044] Where (x, y, z) is the center of gravity of the wire pressing fixture in the force sensor coordinate system, and b1 = T x0 +F y0 ×zF z0 ×y, b2=T y0 +F z0 ×xF x0 ×z, b3=Tz0 +F x0 ×yF y0 ×x. This can be simplified to formula (5) as follows:

[0045]

[0046] By collecting N non-coplanar robot postures (N≥3), and after the robot stabilizes, the force sensor values ​​are read to obtain N sets of force sensor measurement data, which leads to formula (6):

[0047] T=φ·P (6)

[0048] In the formula,

[0049]

[0050] Formula (7) is derived using the least squares method:

[0051] P=(Φ T Φ) -1 Φ T T (7)

[0052] From this, the coordinates (x, y, z) of the center of gravity of the pressure tool in the force sensor coordinate system, as well as b1, b2, and b3, can be calculated.

[0053] 3) Establish a model relating the force sensor and the robot base, with gravity in the world coordinate system O. w The direction vector in is g w =[0 0 -G] T Gravity G, through the transformation moment drop of a, will transform the force sensor coordinate system O. s And by combining formula (2), we can derive formula (8):

[0054]

[0055] in,

[0056] C1=G cos U sinV, C2=-G sin U, C3=-G cos U cos V.

[0057] According to the least squares method, it can be simplified to the following form:

[0058] K = (R T R) -1 R T F (9)

[0059] From this, we can calculate C1, C2, C3, and F. x0 F y0、 F z0According to formulas (2) and (3), the zero-point torque T can be obtained. x0 T y0 T z0 If the installation angles α, β, and γ of the force sensor are known, then the load at point O will be obtained. s Center of gravity (x, y, z) in coordinate system, zero point of force sensor [F] x0 F y0 F z0 T x0 T y0 T z0 ], load weight The robot base tilt angles U = arcsin(-C2 / G) and V = arctan(-C1 / C3), and all other parameters can be calculated.

[0060] S2. Based on the installation deflection angle of the force sensor, design an angle optimization model with the sum of squared errors as the constraint index;

[0061] It should be noted that the gravity compensation model is based on the least squares method, and the purpose of the least squares method is to minimize the sum of squared errors between the obtained data and the actual data. Therefore, the sum of squared errors is an indicator to determine whether the obtained data is closer to reality.

[0062] Step S2 is as follows:

[0063] S21. When there is no external force, the gravity compensation error for the i-th measurement posture of the robotic arm is as follows:

[0064]

[0065] Where δ xi δ yi δ zi δR xi δ Ryi δ Rzi Let F be the gravity compensation error under the i-th measurement attitude. xi F yi F zi T xi T yi T zi Let G be the actual measured value of the force sensor in the i-th measurement posture. xi G yi G zi T gxi T gyi T gzi Let G be the force and torque generated by the gravity of the end effector under the i-th measurement posture. Since the gravity compensation error is mainly determined by the load weight G, the optimization model of this invention only considers the measured force value.

[0066] S22. Design an optimization model for deflection angle based on measured force values:

[0067]

[0068] Where S is the sum of squares of the external force measurement error, and α, β, and γ are the triaxial rotation angles at which the force sensor is installed;

[0069] S23. The optimization objective is to find the values ​​of α, β, and γ that minimize the sum of the squares of the errors. The final deflection angle optimization model is as follows:

[0070] min(S)

[0071] α min ≤Δα≤α max

[0072] β min ≤Δβ≤β max

[0073] γ min ≤Δγ≤γ max

[0074] in Each threshold is a reasonable value set according to the actual situation.

[0075] S3. Under experimental conditions, collect multiple sets of robotic arm pose and force data;

[0076] It should be noted that, under the condition that the end effector of the robotic arm is not subjected to external forces, the movement of the robotic arm covers the key point sequence required by the task. The robot's movement pattern at adjacent path points is linear, and it pauses at each path point for a specified period T. While the robot is collecting path data, it also periodically collects the following data in real time: end effector position data (x1, y1, z1), end effector Euler angle data (R1, P1, Y1), and force data (F) fed back from the six-dimensional force sensor. x1 F y1 F z1 T x1 T y1 T z1 ).

[0077] S4. Based on the improved PSO method, using multiple sets of robotic arm pose and force data under experimental conditions, solve the deflection angle optimization model to obtain the optimal installation deflection angle of the force sensor.

[0078] It should be noted that, assuming α, β, and γ values ​​are 0, the data collected in step a3 is input into the gravity compensation model in step aa1 to obtain preliminary gravity compensation parameters. The deflection angle is set to the three-dimensional particle swarm. Under the deflection angle variation range set in step S2, each particle in the swarm is provided with a random position X.i =[x iα x iβ x iγ and velocity V i =[v ia v iβ v iγ During a particle's flight, its position and velocity are dynamically adjusted based on its own flight experience and that of its companions, ultimately ensuring that all particles in the swarm reach the target's optimal position P. g =[P gα P gβ P gγ ].

[0079] To address the issues of the traditional PSO algorithm's single inertia weight adjustment function not being effective and getting trapped in local optima, a linear decreasing adjustment model is adopted. This model dynamically changes the inertia weight with the number of iterations and introduces a shrinkage learning factor μ to improve the algorithm's search capability and accelerate convergence, as shown in formulas (14) and (15) below:

[0080] W(i) = W max -(W max -W min (i / t) (14)

[0081]

[0082] Where c1 and c2 are learning factors; c1 + c2 > 3, W ma x and W min These are the maximum and minimum values ​​of the preset inertia weights, respectively; t is the current iteration number;

[0083] Among them, the formula (16) for the change in velocity of the particle in each iteration after introducing the shrinkage learning factor and dynamic inertia weight is:

[0084] V ij (t+1)=μ[W(i)·V ij (t)+c1·r1(P ij -x ij (t))+c2·r2(P ij -x ij (t))] (16)

[0085] V ij (t) represents the velocity of the particle after iteration; P ij x represents the optimal position of the particle. ij (t) represents the current position of the particle; r1 and r2 are two independent and uniformly distributed random numbers in the interval (0, 1).

[0086] The flowchart for the improved PSO algorithm to find the optimal solution for the deflection angle is as follows: Figure 5 As shown.

[0087] S5. By installing the force sensor at the optimal installation angle and using the gravity compensation model to perform gravity compensation on the end of the robotic arm under actual working conditions, the accurate contact force of the external environment can be obtained.

[0088] It should be noted that the optimal values ​​of α, β, and γ obtained in step four are then substituted back into the gravity compensation model from step S1 to update the gravity compensation parameters. At this point, the zero-point force value (F) of the six-dimensional force sensor is obtained. x0 F y0 F z0 T x0 T y0 T z0 ) and the force and torque generated by the gravity of the real-time end tool (G) x G y G z T gx T gy T gz The force sensor will measure the value (F) in real time when the live-line working robot's wire-pressing fixture comes into contact with the environment. x F y F z T x T y T z Substituting into Formula 1, the real-time applied force from the external environment (F) is calculated. EXx F EXy F EXz、 T EXx T EXy T EXz ).

[0089] like Figure 6 The diagram shows a comparison of the gravity compensation effects of the method of the present invention and the traditional gravity compensation method, demonstrating that the compensation accuracy of the present invention is significantly improved.

[0090] Please see Figure 7 , Figure 7 This is a schematic diagram of the hardware device in operation according to an embodiment of the present invention. The hardware device specifically includes: a load gravity compensation device 401 for a live-line working robot, a processor 402, and a storage medium 403.

[0091] A device 401: The load gravity compensation device 401 for a live-line working robot implements the load gravity compensation method for a live-line working robot.

[0092] Processor 402: The processor 402 loads and executes the instructions and data in the storage medium 403 to implement the load gravity compensation method for a live-line working robot.

[0093] Storage medium 403: The storage medium 403 stores instructions and data; the storage medium 403 is used to implement the load gravity compensation method for a live-line working robot.

[0094] The key points of this invention are: 1. A load gravity compensation system for live-line working robots is established; 2. To address the problem that traditional methods cannot calculate the compensation error caused by the installation angle of the force sensor, an optimization model is designed to iteratively solve the problem; 3. To address the problems of low efficiency and easy getting trapped in local optima in traditional PSO solutions, a linear decreasing adjustment model and an appropriate shrinkage factor are introduced to quickly and effectively calculate the installation angle.

[0095] The beneficial effects of this invention are as follows: A gravity compensation model is constructed based on the positional relationships of each joint in the robot system; an angle optimization model is designed based on the installation angle of the force sensor, with the sum of squared errors as the constraint index; under experimental conditions, multiple sets of robot arm pose and force data are collected; based on the improved PSO method, the angle optimization model is solved using the multiple sets of robot arm pose and force data under experimental conditions to obtain the optimal installation angle of the force sensor; the force sensor is installed at the optimal installation angle, and under actual working conditions, the gravity compensation model is used to perform gravity compensation on the end effector of the robot arm to obtain accurate external environmental contact force. Compared with traditional gravity compensation methods based on analytical methods, this method has a better load gravity compensation effect because it uses the PSO optimization method to solve for the installation angle of the force sensor; compared with traditional gravity compensation methods based on machine learning, this method can calculate all gravity compensation parameters quickly and efficiently by using a linear decreasing adjustment model and introducing a suitable contraction factor to improve the PSO algorithm, achieving higher accuracy and efficiency in gravity compensation.

[0096] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for compensating for the load gravity of a live-line working robot, characterized in that: Includes the following steps: S1. Construct a gravity compensation model based on the positional relationships of each joint in the robot system; S2. Based on the installation deflection angle of the force sensor, design an angle optimization model with the sum of squared errors as the constraint index; S3. Under experimental conditions, collect multiple sets of robotic arm pose and force data; S4. Based on the improved PSO method, using multiple sets of robotic arm pose and force data under experimental conditions, solve the deflection angle optimization model to obtain the optimal installation deflection angle of the force sensor. S5. By installing the force sensor at the optimal installation angle and using the gravity compensation model to perform gravity compensation on the end of the robotic arm under actual working conditions, the accurate contact force of the external environment can be obtained. In step S4, the PSO algorithm is improved by adjusting the model linearly and designing a shrinkage factor. Step S4 is as follows: S41. Input the data collected in step S3 into the gravity compensation model to obtain preliminary gravity compensation parameters, and set the force sensor installation angle to a three-dimensional particle swarm. S42, Randomize the position X of each particle under the constraint of the bias angle i = [x iα , x iβ , x iγ ] and velocity V i = [v iα , v iβ , v iγ ]; S43, the linear decreasing adjustment model is adopted, and a contraction learning factor μ is introduced to improve the flying of the whole population particles to the target optimal position P g = [P gα , P gβ , P gγ ] is the velocity, and is specifically as follows: W(i) = W max - (W max - W min ) · (i / t); ； wherein c1, c2 are learning factors; c1 + c2 > 3, W max and W min are the maximum and minimum values of the preset inertia weight respectively; t is the current iteration number; the velocity formula of each iteration of the particle after introducing the contraction learning factor and the dynamic inertia weight is as follows: V ij (t+1) = μ [W(i) · V ij (t) + c1 · r1(P ij -x ij (t)) + c2 · r2(P ij -x ij (t))], wherein V ij (t) is the velocity of the particle after iteration; P ij is the optimal position of the particle; x ij (t) is the current position of the particle; r1 and r2 are two random numbers independently and uniformly distributed in the interval (0, 1). S44. When the iterative calculation conditions are met, the deflection angle optimization model converges, and the optimal solution for the deflection angle is obtained. Substitute the optimal solution of the deflection angle back into the gravity compensation model, update the gravity compensation parameters, and obtain the final applied force from the external environment based on the calculation formula of the force sensor applied by the external environment in the coordinate system.

2. The load gravity compensation method for a live-line working robot as described in claim 1, characterized in that: In step S1, the construction process of the gravity compensation model is as follows: S11, obtain the zero point data F0=(F x0 , F y0 , F z0 , T x0 , T y0 , T z0 ) of the six-dimensional force sensor, the force and torque generated by the gravity of the end tool G=(G x , G y , G z , T gx , T gy , T gz ), the force and torque applied by the external environment F EX =(F EXx , F EXy , F EXz , T EXx , T EXy , T EXz ); S12. Based on the parameters in step S11, obtain the force applied by the force sensor to the external environment in the coordinate system: F EX = F - F0 - G; where F = (F x , F y , F z , T x , T y , T z ) are the forces and moments measured by the six-dimensional force sensor.

3. The load gravity compensation method for a live-line working robot as described in claim 1, characterized in that: Step S2 is as follows: S21. When there is no external force, the gravity compensation error for the i-th measurement posture of the robotic arm is as follows: ； where δ xi , δ yi , δ zi , δ Rxi , δ Ryi , δ Rzi , δ xi , δ yi , δ zi , δ xi , δ yi , δ zi , δ xi , δ yi , δ zi , δ gxi , δ gyi , δ gzi is the gravity compensation error at the ith measurement pose, F xi , F yi , F zi , F gxi , F gyi , F gzi is the actual measurement value of the force sensor at the ith measurement pose, G xi , G yi , G zi , G gxi , G gyi , G gzi is the force and torque generated by the end tool gravity at the ith measurement pose; S22. Design an optimization model for deflection angle based on measured force values: ； Where S is the sum of squares of the external force measurement error, and α, β, and γ are the triaxial rotation angles at which the force sensor is installed; S23. The optimization objective is to find the values ​​of α, β, and γ that minimize the sum of the squares of the errors. The final deflection angle optimization model is as follows: ； in Each threshold is a reasonable value set according to the actual situation.

4. The load gravity compensation method for a live-line working robot as described in claim 1, characterized in that: Step S3 is as follows: Under experimental conditions, the following data are collected periodically in real time: end-effector position data (x1, y1, z1), end-effector Euler angle data (R1, P1, Y1), and force data (F) fed back by the six-dimensional force sensor. x1 F y1 F z1 T x1 T y1 T z1 ).

5. A storage medium, characterized in that: The storage medium stores instructions and data to implement the load gravity compensation method for a live-line working robot as described in any one of claims 1 to 4.

6. A load gravity compensation device for a live-line working robot, characterized in that: include: A processor and a storage medium; the processor loads and executes instructions and data in the storage medium to implement the load gravity compensation method for a live-line working robot as described in any one of claims 1 to 4.

Images