A current-optimization-based mechanical arm joint gravity compensation system

Abstract

The application discloses a mechanical arm joint gravity compensation system based on current optimization, which is composed of a first angle sensor, a second angle sensor, a matrix transformation module, a position sensor, a centroid vector module, a gravity compensation calculation module, a gravity vector module, an initial compensation module, a first current sensor, a second current sensor, an interpolation current calculation module and a particle optimization module. A D-H parameter table is established according to the structural parameters of the mechanical arm, a coordinate transformation matrix is obtained, the gravity vector and the gravity moment of all joints on the mechanical arm are obtained from the coordinate transformation matrix and the physical parameters of the mechanical arm, the initial current value is obtained from the gravity vector and the gravity moment, the real-time optimal compensation current is obtained through the improved particle swarm optimization, the gravity compensation values of the joints are corrected, the problem of compensation value deviation is solved, the compensation data can be adjusted in time when the load changes or the external environment changes, the operation of the mechanical arm is more stable, and the working efficiency of the mechanical arm is improved.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of robot compensation, and particularly relates to a system capable of realizing mechanical arm gravity compensation by optimizing current. BACKGROUND

[0002] A robot is an intelligent mechanical device designed and manufactured based on human actions as a template. Robots have expanded from initial industrial applications to various fields, from manufacturing, agriculture, and industry to service industries, and robots can be found in all walks of life, gradually becoming an indispensable part of modern society. The mechanical arm of a robot is usually composed of multiple joints and connecting pieces. The load may change at different positions and postures, which causes the center of gravity position and stress condition of the mechanical arm to change. Due to the action of the earth's gravity, the load of the mechanical arm is affected by gravity, which causes the mechanical arm to deviate in posture, become unstable, or fail to accurately reach the target position during movement.

[0003] In the control and operation of a mechanical arm system, gravity compensation is a very important technology. Gravity compensation can significantly reduce the load of each joint, connecting rod, and load of the mechanical arm on the motor and structure of the mechanical arm, help the mechanical arm overcome the influence of gravity, enable it to more accurately perform the required tasks and trajectories, and improve production efficiency and product quality. In addition, gravity compensation can also reduce the energy consumption of the mechanical arm during movement, prolong the service life of the equipment, and reduce maintenance costs. For example, the document with the Chinese patent publication number CN116922399 proposes a gravity compensation method based on Kalman filtering and mechanical arm zero gravity compensation. The method obtains the optimal moment state and optimal gravity compensation variance through Kalman filtering gain, but the gravity compensation current data is only obtained by interpolation, and the resulting result is not necessarily the optimal current compensation, so the compensation accuracy is not high. SUMMARY

[0004] The purpose of the present application is to solve the problems existing in the gravity compensation of the existing mechanical arm, and to provide a mechanical arm joint gravity compensation system based on current optimization with optimal current compensation, which has high compensation accuracy and improves the running stability of the mechanical arm.

[0005] The technical scheme adopted by the mechanical arm joint gravity compensation system based on current optimization is as follows: it is composed of a first angle sensor, a second angle sensor, a matrix transformation module, a position sensor, a center of mass vector module, a gravity compensation calculation module, a gravity vector module, an initial compensation module, a first current sensor, a second current sensor, an interpolation current calculation module, and a particle optimization module. The basic mechanical parameters of the mechanical arm in the initial state are obtained by measurement and collection of the first angle sensor, and the basic mechanical parameters are input into the matrix transformation module to obtain a transformation matrix The center of mass coordinates X of each connecting rod are obtained by the position sensori Y i Z i and the x-coordinate of the position where gravity acts. g(i) and the vertical axis Y g(i) , where i is the number of links or joints in the robotic arm; the centroid coordinate X of each link is... i Y i Z i The relative centroid vector P is obtained by inputting it into the centroid vector module. i Mass m of each link i With the aforementioned transformation matrix Relative centroid vector P i The gravitational torque ME is obtained by inputting it into the gravity compensation calculation module. i The x-coordinate of the location where gravity acts. g(i) , ordinate Y g(i) and gravitational torque ME i The gravity vector is obtained by inputting it into the gravity vector module. Real-time current I is detected by the first current sensor in The real-time working angle φ of each joint of the robotic arm is detected by the second angle sensor. i , the gravity vector Real-time current I in Real-time working angle φ i And the inertia matrix M(r) and the Coriolis force and centrifugal force matrices The initial compensation current I is obtained by inputting it into the initial compensation module. com The second current sensor detects the measured currents I1, I2, and I4 at the first, second, and fourth joints. The corresponding joint selection angles Ai, Bj, and Ck, and the working angle ranges A, B, and C are used as input signals to the interpolation current calculation module to obtain the initial current value I. org The initial value of the current I org With the set operating current I set The optimal current compensation I is obtained as input to the particle optimization module. g Optimal current compensation I g and initial compensation current I com The summation yields the final compensation current I. final This compensates for the gravity of the robotic arm joints.

[0006] The beneficial effects of this invention are:

[0007] 1. The gravity compensation method provided by the application is to compensate the gravity of the mechanical arm by optimizing and improving the particle swarm algorithm, solves the problem of compensation value deviation of the existing gravity compensation method, can adjust the compensation data in time when the load changes or the external environment changes, makes the operation of the mechanical arm more stable, and improves the working efficiency of the mechanical arm.

[0008] 2. The particle swarm optimization in the application reduces the inertia weight while increasing the iteration number and introduces a contraction factor, so that the algorithm has strong global convergence ability in the early iteration stage and strong local convergence ability in the later iteration stage. It is a real-time optimization that can ensure the stable operation and operation accuracy of the mechanical arm and improve the working efficiency of the mechanical arm.

[0009] 3. The particle swarm algorithm is used to obtain the global optimal current compensation, but the inertia weight of most particle swarm algorithms is a constant value, so the global and local convergence ability of the particles needs to be improved.

[0010] 4. The interpolation current calculation in the application is a real-time calculation that improves the resolution of data and effectively enhances the accuracy of current data without increasing the sampling frequency.

[0011] 5. The application constructs a D-H coordinate according to the structural parameters of the mechanical arm and establishes a D-H parameter table, obtains a coordinate transformation matrix from the D-H parameter table, obtains the gravity vector and gravity torque of all joints of the mechanical arm from the coordinate transformation matrix and the physical parameters of the mechanical arm, obtains the initial current value from the gravity vector and the gravity torque, and obtains the real-time optimal compensation current by the improved particle swarm optimization. The data of the gravity compensation value of each joint is corrected, so that the compensation and correction accuracy is high. BRIEF DESCRIPTION OF DRAWINGS

[0012] The application will be further described in detail below in combination with the drawings and specific embodiments.

[0013] Figure 1 The figure is a schematic diagram of the structure of a six-axis mechanical arm controlled by the mechanical arm joint gravity compensation system of the application and its space coordinate system.

[0014] Figure 2 The figure is a structural block diagram of the mechanical arm joint gravity compensation system of the application.

[0015] Figure 3 The figure is a control flowchart of the mechanical arm joint gravity compensation system shown in the figure. Figure 2

[0016] ​In the figure: 31. First angle sensor; 32. Matrix transformation module; 33. Position sensor; 34. Centroid vector module; 35. Gravity compensation calculation module; 36. Gravity vector module; 37. Initial compensation module; 38. First current sensor; 39. Interpolation current calculation module; 40. Second current sensor; 41. Particle optimization module; 42. Second angle sensor. Detailed Implementation

[0017] like Figure 1 The six-axis robotic arm shown has a base and six sequentially connected links 1, 2, 3, 4, 5, and 6. Six joints are also provided between the base and the connections of the six links. In the initial state of the six-axis robotic arm, the spatial coordinate systems of the base and each robotic arm are established sequentially, denoted as O... i (x i ,y i ,z iThe number of links or joints in the robotic arm is i = 0, 1, 2...6. The system comprises the following components: The first joint connects the first link 1 and the base. The origin of the spatial coordinate system O0 is at the geometric center of the horizontal plane of the base. The z0 axis is the axial direction of the first joint, pointing towards the center of the first link 1. The x0 and y0 axes are on the horizontal bottom surface of the base. The second joint connects the first link 1 and the second link 2. The z1 axis of the spatial coordinate system O1 is the axial direction of the second joint, pointing towards the second link 2. The origin is the intersection of the horizontal plane containing the center of mass of the first link 1 and the z0 axis. The x1 axis is in the same direction as the x0 axis, and the y1 axis is in the same direction as the z0 axis. The third joint connects the second link 2 and the third link 3. The origin of the spatial coordinate system O2 is the intersection of the plane x0Oy0 formed by the coordinate axes x0 and z0 and the axis of the third joint. The z2 axis is the axial direction of the third joint, pointing towards the second link 2. The y2 axis is in the same direction as the x1 axis, and the x2 axis is opposite to the y1 axis. The fourth joint connects the third link... Links 3 and 4 are connected by a spatial coordinate system. The origin of spatial coordinate system O3 is the intersection of the plane x0Oy0 formed by coordinate axes x0 and z0 and the axis of the fourth joint. The coordinate axes of O3 and O2 are in the same direction. The origin of spatial coordinate system O4 is the geometric center of the fourth link 4. The direction of the y4 axis is the same as the z3 axis, the direction of the x4 axis is opposite to the y3 axis, and the direction of the z4 axis is the axis of the fifth joint and points towards the fifth link 5. The fifth joint connects the fourth link 4 and the fifth link 5. The origin of spatial coordinate system O5 is the intersection of the z4 axis and the horizontal plane passing through the axis of the sixth joint. The direction of the z5 axis is the axis of the sixth joint and points towards the sixth link 6. The direction of the x5 axis is the same as the x4 axis, and the direction of the y5 axis is opposite to the z4 axis. The sixth joint connects the fifth link 5 and the sixth link 6. The origin of spatial coordinate system O6 is the intersection of the end plane of the sixth link 6 and the z5 axis. The coordinate axes of O6 and O5 are in the same direction.

[0018] This invention is based on Figure 1 Taking the six-axis robotic arm shown as an example, the performance parameters of the robotic arm, including the mass m of each link, can be obtained by consulting relevant manuals. i (i = 1, 2…6), the operating current I of the robotic arm set The robotic arm manufacturer calculates the robotic arm's inertia matrix M(r), Coriolis force, and centrifugal force matrices based on CAD design. The working angle ranges of the first joint (A), the second joint (B), and the fourth joint (C) of the robotic arm are measured. Two basic parameters of the robotic arm, a, can be obtained through measurement. i and d i That is, in the initial state of the six-axis robotic arm, the origins O of two adjacent spatial coordinate systems are... i (x i ,y iz i horizontal or vertical distance between the coordinate axis z i-1 and the coordinate axis z i along the coordinate axis x i is a i , the coordinate axis x i-1 and the coordinate axis x i along the coordinate axis z i-1 is d i .

[0019] As shown in Figure 2 , the mechanical arm joint gravity compensation system of the application is composed of angle sensors 31, 42, a matrix transformation module 32, a position sensor 33, a centroid vector module 34, a gravity compensation calculation module 35, a gravity vector module 36, an initial compensation module 37, current sensors 38, 40, an interpolation current calculation module 39, and a particle optimization module 41. Among them, the first angle sensor 31, the matrix transformation module 32 and the gravity compensation calculation module 35 are connected in sequence, the position sensor 33, the centroid vector module 34 and the gravity compensation calculation module 35 are connected in sequence, the output end of the position sensor 33 is also connected to the input end of the gravity vector module 36, the output end of the gravity compensation calculation module 35 is connected to the gravity vector module 36 and the initial compensation module 37 in sequence, the second current sensor 40, the interpolation current calculation module 39 and the particle optimization module 41 are connected in sequence, and the output ends of the first current sensor 38 and the second angle sensor 42 are connected to the initial compensation module 37. The basic mechanical parameters of the mechanical arm are input into the matrix transformation module 32 to obtain the transformation matrix The centroid vector module 34 obtains the relative centroid vector P i from the position parameters X i , Y i and Z i input by the sensor The transformation matrix i , the relative centroid vector P i and the link mass m i are input into the gravity compensation calculation module 35 to output the gravity moment ME g(i) , X g(i) , X i , Y i and the gravity moment ME i are input into the gravity vector module 36 to obtain the gravity vector The gravity vector is input into the initial compensation module 37 together with the real-time current I in , the inertia matrix M(r) and the Coriolis force and centrifugal force matrix to obtain the initial compensation current I comThe measured currents I1, I2, I4 of the first, second, and fourth joints, the joint selection angles Ai, Bj, Ck, and the working angle ranges A, B, C of the first, second, and fourth joints are input as input signals to the interpolation current calculation module 39 to calculate the initial current value I. org Compare it with the set operating current I set The optimal current compensation I is obtained by inputting the signal into the particle optimization module 41. g Optimal current compensation I g and initial compensation current I com The final compensation current I can be obtained by adding them together. final This involves compensating for the gravity of the robotic arm joints. Specifically:

[0020] The first angle sensor 31 is used to acquire the basic parameters of the other two robotic arms in the initial state of the six-axis robotic arm, namely the coordinate axes z of the two adjacent spatial coordinate systems from the base to each robotic arm. i-1 With coordinate axis z i The included angle α i And obtain the x-axis i-1 with coordinate axis x i The included angle θ i .

[0021] Distance a i d i and included angle α i θ i Four signals are input to the matrix transformation module 32, and the output signal of the matrix transformation module 32 is the previous coordinate system O between two adjacent spatial coordinate systems. i-1 (x i-1 ,y i-1 ,z i-1 ) to the next coordinate system O i (x i ,y i ,z i Transformation matrix Here, i ≠ 0.

[0022] Combined Figure 3 As shown, the matrix transformation module 32 establishes the DH coordinate system based on the input parameter signal. The DH parameter table is shown in Table 1 below:

[0023] Table 1

[0024] i a i ]]> d i ]]> i ]]> ​ i ]]> ​ 1 0 d1 -90° [theta1] 2 [a2] 0 0 [theta2] 3 [a3] 0 0 [theta]3 4 0 0 90° [theta]4 5 0 [d5] -90° [theta]5 6 0 [d6] 0 [theta]6

[0025] From the table of the standard DH parameter method, we can obtain the values ​​from coordinate system O. i-1 (x i-1 ,y i-1 ,z i-1) to the coordinate system O i (x i ,y i ,z i ) transformation matrix transformation matrix can be derived from equation (1):

[0026]

[0027] wherein, are the transformation matrices of the six axes, whose values are derived from equations (2)-(7):

[0028]

[0029] The position sensor 33 detects the position of each link, i.e. the centroid coordinates X i , Y i , Z i , and also detects the horizontal coordinate X g(i) and the vertical coordinate Y g(i) of the gravity action position of each link. The centroid coordinates of each link are input into the centroid vector module 34, which calculates the relative centroid vector P i P i is derived from equation (8):

[0030] P i = [X i Y i Z i 1] T (i = 1, 2...6) (8)

[0031] wherein X i represents the horizontal coordinate of the centroid of the ith link, Y i represents the vertical coordinate of the centroid of the ith link, Z i represents the vertical coordinate of the centroid of the ith link, and T represents the matrix transpose.

[0032] The transformation matrix T output by the matrix transformation module 32, the mass m i of each link (i = 1, 2...6) and the relative centroid vector P i output by the centroid vector module 34 are input into the gravity compensation calculation module 35 as input signals. The gravity compensation calculation module 35 calculates the gravity torque ME ij ME ij represents the gravity torque of the jth link acting on the ith joint, whose value is derived from equations (9)-(14).

[0033]

[0034] ME 66 =0 (14)

[0035] Where g is the acceleration due to gravity.

[0036] The gravitational torque ME acting on each joint of the first to sixth links 6. ij Adding them together, we get the total gravitational torque ME of each joint. i For example, ME1 represents the total gravitational torque acting on the first joint, the value of which can be obtained from formula (15):

[0037]

[0038] The total gravitational torque ME acting on each joint i The x-coordinate of the centroid of the i-th link is X. i and the vertical axis Y i The x-coordinate of the position where gravity acts g(i) and the vertical axis Y g(i) The input signal is fed into the gravity vector module 36, where the x-coordinate of the center of mass of the i-th link is X. i and the vertical axis Y i The x-coordinate of the position where gravity acts g(i) and the vertical axis Y g(i) Data collected by position sensor 33.

[0039] Gravity vector module 36 first calculates the perpendicular vector from the center of mass to the point of application of gravity, i.e., the lever arm R, which is obtained from formula (16):

[0040]

[0041] Based on the lever arm R and the total gravitational torque ME i Obtain the gravity vector of each joint. From formula (17):

[0042]

[0043] Finally, based on the gravity vector of each joint Obtain the gravity vector Its value is derived from formula (18):

[0044]

[0045] in, The gravity vector for each joint.

[0046] The real-time working angle φ of each joint of the robotic arm is detected by the second angle sensor 42. i(i = 1, 2…6), the real-time current of each joint I is detected by the first current sensor 38 in , the gravity vector , the inertia matrix M(r), the real-time working angle of each joint φ i (i = 1, 2…6), the Coriolis force and centrifugal force matrix The real-time current of each joint I in is input into the initial compensation module 37. The initial compensation module 37 calculates the joint torque vector τ according to formula (19), which is also the output torque dynamics equation:

[0047]

[0048] Where τ is the joint torque vector, r is the joint position vector, and the joint position vector r can be obtained by formula (20), is the joint velocity vector, is the joint acceleration vector, and T is the matrix transpose:

[0049] r = [φ1 φ2 φ3 φ4 φ5 φ6] T (18)

[0050] The output torque of the mechanical arm, the torque constant K α The relationship between the motor input current and the torque constant K can be obtained by formula (21):

[0051]

[0052] The initial compensation current I of the mechanical arm can be obtained by the gravity vector and the torque constant K α The value of I can be obtained by formula (22): com

[0053]

[0054] The measured currents I1, I2, I4 of the first joint, the second joint, and the fourth joint are obtained by the current sensor 40, and the joint selection angles are Ai, Bj, Ck (i, j, k ∈ {1, 2, 3}), the measured currents I1, I2, I4, the joint selection angles Ai, Bj, Ck, and the joint working range angles A, B, C are input into the interpolation current calculation module 39 as input signals, where I1, I2, I4 are the measured currents of the first joint, the second joint, and the fourth joint, respectively, obtained by the current sensor 40, and the joint selection angles Ai, Bj, Ck are within the value range of the corresponding joint working range angles A, B, C.

[0055] ​The interpolation calculation is performed using the first, second, and fourth joints. In the working angle ranges A, B, and C of the first, second, and fourth joints, three angle values are selected at each joint as selection angles, denoted as A1-A3, B1-B3, and C1-C3. A1-A3 represent the selection position angles of the first joint, B1-B3 represent the selection position angles of the second joint, and C1-C3 represent the selection position angles of the fourth joint. The three joints can be combined to form a total of 27 measurement positions P Ai,Bj,Ck The measurement position P Ai,Bj,Ck can be obtained by formula (23):

[0056] P Ai,Bj,Ck =(Ai,Bj,Ck) (i,j,k∈{1,2,3}) (21)

[0057] In the 27 measurement positions, the current data I i,j,k is obtained after data acquisition by the current sensor of the motor of each axis. The value of I i,j,k can be obtained by formula (24):

[0058]

[0059] The current data I i,j,k is subjected to interpolation processing to obtain the current initial value I org . The value of I org can be obtained by formula (25):

[0060]

[0061] where μ1 is the first-order interpolation quotient and μ2 is the second-order interpolation quotient.

[0062] The current initial value I org and the set operating current I set are input as input signals to the particle optimization module 41. The particle optimization module 41 performs particle swarm optimization (PSO) optimization algorithm processing, specifically:

[0063] 1. Define the basic parameters of the particle swarm:

[0064] Define the number of particles num=50; the number of iterations nums is 100; the maximum inertia weight ω max =1.5, the minimum inertia weight ω min =0.9, the inertia weight ω; the acceleration factor 1 is C1, the acceleration factor 2 is C2; the parameter dimension is 1 dimension.

[0065] The inertia weight omega in the traditional PSO algorithm is a fixed value, and the unchangeable inertia weight has a disadvantage in global search, and the omega is not large enough to jump out of the local extremum and fall into the local optimal solution, and if the omega is too large in local search, the optimal solution cannot be quickly converged. In order to solve the problem, the omega in the application adopts an adaptive adjustment strategy, that is, the value of omega is linearly reduced with the iteration number. With the increase of the iteration number, the inertia weight omega is linearly reduced, so that the particle swarm algorithm has strong global convergence ability in the early stage and strong local convergence ability in the later stage. The adjustment strategy of omega is obtained from formula (26):

[0066]

[0067] Wherein, nums k represent the current iteration number.

[0068] 2, initialize the speed and position information of particles

[0069] a) the number of position information definition particle group is 50, and the dimension is 1 dimension;

[0070] b) the speed information is initialized as a random value (that is, the moving direction and speed are random).

[0071] 3, initialize the global best position and the best fitness of the particle group

[0072] a) the initial position of the particle is set as the individual best position P BEST ;

[0073] b) the initial best fitness of the particle is set as a zero vector;

[0074] 4, initialize the global best position and fitness of the particle group

[0075] a) the global best position G BEST of the particle is initially set as a zero vector;

[0076] b) the global best fitness is set as infinity;

[0077] 5, evaluate the initial fitness of the particle group.

[0078] a) traverse all particles;

[0079] b) the fitness of the current particle is calculated by the fitness function, and then the best fitness of each particle (that is, the best fitness that each particle can reach) is updated. The definition of the fitness function is in step 8;

[0080] c) compare the fitness of the current particle with the global best fitness;

[0081] d) if the fitness of the current particle is better than the global best fitness, then update the best fitness to the fitness of the current particle and update the global best position to the current position of the particle;

[0082] By this method, the particles can be gradually gathered to the global best position, and the search of the optimization target is performed.

[0083] 6. Particle swarm optimization main program:

[0084] Outer loop: control the iteration number to be 100;

[0085] Inner loop: the loop is traversed through all the particles, and the number of particles is 50;

[0086] Update the speed of the particle: mainly composed of the following three parts.

[0087] 1) Inertia part: make the particle keep the present motion trend.

[0088] 2) Individual part: guide the particle to approach the individual particle best position.

[0089] 3) Group part: guide the particle to approach the global particle best position.

[0090] The speed V of the particle of the traditional PSO particle swarm algorithm k and the position P k can be obtained by formula (27):

[0091]

[0092] Wherein, V k represents the particle speed after iteration k times; C1 and C2 represent acceleration factors; r1 and r2 represent random numbers, and the value is between 0-1; P k represents the particle position after k iterations; represents the historical optimal position of the particle τ in the kth iteration, that is, the optimal solution searched by the τth particle after the kth iteration; represents the historical optimal position of the group in the kth iteration, that is, the optimal solution in the whole particle group after the kth iteration.

[0093] The speed update of the particle is usually affected by the inertia weight and the acceleration factor, which may cause the speed to be too large or too small in the search process, and further affect the balance of the global search and local search ability.

[0094] On the basis of the traditional PSO, the contraction factor is added to improve the search performance and convergence characteristics of the algorithm. The contraction factor The particle velocity of the improved PSO algorithm can be obtained from equation (29) according to equation (28):

[0095]

[0096] wherein C=C1+C2.

[0097]

[0098] Boundary setting: limit the position of the particle to be no less than 0. If it is less than 0, reset its value to 0;

[0099] Calculate fitness: use the evaluate_fitness function, which receives the current particle position and velocity as parameters, and returns the corresponding fitness value.

[0100] Update individual best fitness and best position: if the current calculated fitness value is better (smaller value) than the individual best fitness value recorded by the particle before, then update the individual best fitness value and the corresponding position.

[0101] Update global best fitness and best position: if the current calculated fitness value is better (smaller value) than the global best fitness value, then update the global best fitness value and the corresponding position.

[0102] 7. Display the current iteration best fitness: update the information of the current iteration after each iteration, and display the optimal parameters and output the optimal current compensation I g .

[0103] 8. Define the fitness function: define the fitness function fitness from equation (30):

[0104] g(P k ,V k ,I org ) = fitness + I set (28)

[0105] wherein g(P k ,V k ,I org ) is the current model function, which is generally obtained from the parameters of the working robot arm and empirical formula.

[0106] After the particle swarm algorithm, the optimal fitness can be obtained, and the optimal current compensation I g can be obtained by calculation, and the optimal current compensation is obtained from equation (31):

[0107] I g = g(P k ,Vk ,I org )-I set (29)

[0108] The final compensation current I final can be obtained by the initial compensation current and the optimal current compensation, and the value can be obtained by formula (32):

[0109] I final = I com + I g (30)

[0110] The final compensation current I final calculated is input as a feedforward input into the current loop, which increases the operation accuracy and stability of the mechanical arm.

[0111] The above is only the preferred embodiment of the present application, and does not limit the scope of the present application, and any equivalent structure or equivalent flow transformation or direct or indirect application in other related technical fields by using the content of the specification and drawings of the present application is also included in the protection scope of the present application.

Claims

1. A gravity compensation system for a robotic arm joint based on current optimization, characterized in that: consisting of the first and second angle sensors (31, 42), the matrix transformation module (32), the position sensor (33), the center of mass vector module (34), the gravity compensation calculation module (35), the gravity vector module (36), the initial compensation module (37), the first and second current sensors (38, 40), the interpolated current calculation module (39), and the particle optimization module (41), The basic mechanical parameters of the robotic arm in its initial state are obtained by measurement and acquisition by the first angle sensor (31). These basic mechanical parameters are then input into the matrix transformation module (32) to obtain the transformation matrix. The center-of-mass coordinate X of each link is obtained by the position sensor (33). i Y i Z i and the x-coordinate of the position where gravity acts. g(i) and the vertical axis Y g(i) , where i is the number of links or joints in the robotic arm; the centroid coordinate X of each link is... i Y i Z i The relative centroid vector P is obtained by inputting it into the centroid vector module (34). i Mass m of each link i With the aforementioned transformation matrix Relative centroid vector P i The gravitational torque ME is obtained by inputting it into the gravity compensation calculation module (35). i The x-coordinate of the location where gravity acts. g(i) , ordinate Y g(i) and gravitational torque ME i The gravity vector is obtained by inputting it into the gravity vector module (36). The real-time current I is detected by the first current sensor (38). in The real-time working angle φ of each joint of the robotic arm is detected by the second angle sensor (42). i , the gravity vector Real-time current I in Real-time working angle φ i And the inertia matrix M(r) and the Coriolis force and centrifugal force matrices The initial compensation current I is obtained by inputting it into the initial compensation module (37). com ; The first, second and fourth joint measured currents I1, I2 and I4 are detected by the second current sensor (40), and the corresponding joint selection angles Ai, Bj and Ck and working angle ranges A, B and C are input as input signals into the interpolation current calculation module (39) to obtain a current initial value I org The current initial value I org is added to the set operating current I set to obtain an optimal current compensation I g The optimal current compensation I g is added to the initial compensation current I com to obtain a final compensation current I final , and the mechanical arm joint gravity is compensated. The particle optimization module (41) performs a particle swarm optimization algorithm process on the current initial value I org with a set operating current I set The particle swarm optimization algorithm has an inertia weight nums k representing the current iteration number, nums representing the iteration number, ω max max representing the maximum inertia weight, and ω min min representing the minimum inertia weight The velocity of the particle in the particle swarm optimization algorithm V k is the velocity of the particle after k iterations, C1, C2 are acceleration factors, r1 and r2 are random numbers, and take values between 0-1; P k is the position of the particle after k iterations, is the historical optimal position of the particle τ in the kth iteration, is the historical optimal position of the group in the kth iteration, and the contraction factor C = C1 + C2.

2. The current-optimization-based mechanical arm joint gravity compensation system according to claim 1, characterized in that: Establish spatial coordinate systems for the base and each robotic arm, respectively. The basic mechanical parameters include the coordinate axes z of the two adjacent spatial coordinate systems between the base and each robotic arm in the initial state. i-1 With coordinate axis z i The included angle α i coordinate axis x i-1 with coordinate axis x i The included angle θ i coordinate axis z i-1 With coordinate axis z i Along the x-axis i distance a i coordinate axis x i-1 With coordinate axis x i Along the z-axis i-1 distance d i The matrix transformation module (32) establishes the DH coordinate system and obtains the coordinate system O. i-1 (x i-1 ,y i-1 ,z i-1 ) to coordinate system O i (x i ,y i ,z i The transformation matrix described above 3. The current-optimization-based mechanical arm joint gravity compensation system according to claim 1, characterized in that: The centroid vector module (34) is given by the formula P i = [X i Y i Z i 1] T The relative centroid vector P i is obtained, T being the matrix transpose.

4. The current-optimization-based mechanical arm joint gravity compensation system according to claim 3, wherein the gravity The vector module (36) first calculates the force vector Then calculates the gravity vector for each joint Finally, the gravity vector is obtained For each joint 5. The current-optimization-based mechanical arm joint gravity compensation system according to claim 4, characterized in that: The initial compensation module (37) calculates the joint torque vector τ according to the formula The joint position vector r = [φ1 φ2 φ3 φ4 φ5 φ6] is calculated T , The joint velocity vector v is calculated The joint acceleration vector a is calculated, and T is the matrix transpose: according to the formula The torque constant K is calculated α The initial compensation current of the robot arm is obtained 6. The current-optimization-based mechanical arm joint gravity compensation system according to claim 1, wherein In the working angle range A, B, C of the first, second, and fourth joints, three angle values are selected as selection angles at each joint, and the measurement positions of the three joints combine 27 measurement positions. After data acquisition of the current sensor of the motor of each axis in the 27 measurement positions, current data I is obtained i,j,k The current data I i,j,k is subjected to interpolation processing to obtain the current initial value I org .

7. The current-optimization-based mechanical arm joint gravity compensation system according to claim 1, wherein: The fitness in the particle swarm optimization algorithm is calculated using the evaluate_fitness function, and the fitness function fitness is g(P k , V k , I org ) = fitness + I set , and g(P k , V k , I org ) is a current model function obtained from parameters of the working robot arm and an empirical formula.

8. The current-optimization-based mechanical arm joint gravity compensation system according to claim 7, characterized in that: optimal current I g = g(P k , V k , I org )-I set , final compensation current I final = I com + I g .

Images