Gravity compensation method for mechanical arm

Abstract

The invention provides a gravity compensation method of a mechanical arm, which comprises the following steps: when a deflection joint motor is at different deflection joint angles, controlling a parallel linkage joint to do reciprocating constant-speed motion, and collecting output torque data of a parallel joint motor in real time; carrying out optimization processing on the collected output torque data to obtain a gravity baseline data set of the parallel linkage joints; according to the gravity baseline data set, calculating a compensation gravitational torque for gravity compensation of the parallel linkage joint; the technical problems that in the prior art, a dynamic modeling method not only needs accurate mechanical parameters and a complex calculation process, but also needs a large amount of experimental data to calibrate a calculation result, and the cost and time consumption in the compensation torque calculation process are increased are solved. Therefore, the response time of the mechanical arm to the external environment change is prolonged, and the compensation effect of the compensation torque is further affected.

Description

Technical Field

[0001] The present invention relates to the technical field of force compensation, and in particular to a gravity compensation method for a robotic arm. Background Art

[0002] With the development of artificial intelligence technology, more and more researchers have begun to explore the use of neural networks to achieve gravity compensation of robotic arms in terms of pattern recognition and nonlinear system modeling.

[0003] In terms of specific technical implementation, some research has used genetic algorithms to optimize neural networks to achieve gravity compensation in robotic arms. For example, a theoretical formula for calculating the gravity term for the torque of each joint of a robotic arm was derived using the DH parameter modeling method, and the correctness of the theoretical formula was verified using SolidWorks simulation software. The neural network optimized by the genetic algorithm was then used to predict the gravity term, reducing computational complexity and improving efficiency. However, this method essentially uses a dynamic model to calculate the compensation torque.

[0004] However, there is at least one of the following problems in the related technology: in actual applications, the robotic arm needs to respond quickly to changes in the external environment. However, the dynamic modeling method in the existing technology not only requires precise mechanical parameters and complex calculation processes, but also requires a large amount of experimental data to calibrate the calculation results, which increases the cost and time consumption in the compensation torque calculation process, thereby prolonging the response time of the robotic arm to changes in the external environment, and thus affecting the compensation effect of the compensation torque. Summary of the Invention

[0005] The technical problem solved by the present invention is that the dynamic modeling method in the existing technology not only requires precise mechanical parameters and complex calculation processes, but also requires a large amount of experimental data to calibrate the calculation results, which increases the cost and time consumption in the compensation torque calculation process, thereby prolonging the response time of the robotic arm to changes in the external environment, and further affecting the compensation effect of the compensation torque.

[0006] To solve the above technical problems, the present invention provides a gravity compensation method for a robotic arm. The robotic arm includes a parallel linkage joint, an end slide shaft connected to the parallel linkage joint, and a parallel joint motor and a yaw joint motor for driving the parallel linkage joint. The gravity compensation method includes: When the yaw joint motor is at different yaw joint angles, the parallel linkage joint is controlled to move back and forth at a uniform speed, and the output torque data of the parallel joint motor is collected in real time; Optimize the collected output torque data to obtain the gravity baseline data set of the parallel linkage joint; The compensation gravity torque for the gravity compensation of the parallel linkage joint is calculated according to the gravity baseline data set.

[0007] Compared with the existing technology, the technical effect achieved by adopting this technical solution is: this solution collects the output torque data of the parallel joint motor in real time, does not require complex calculations of a large number of mechanical parameters, and avoids the complex dynamic modeling process, simplifies the calculation process of compensating gravity torque, thereby reducing the cost in the gravity compensation process and reducing time consumption, thereby shortening the response time of the robotic arm to changes in the external environment and improving the compensation effect of compensating gravity torque.

[0008] In one embodiment of the present invention, the collected output torque data is optimized to obtain a gravity baseline dataset of a parallel linkage joint, including: Classify and process the output torque data; Calculating the average value of the output torque data after the classification process to obtain average torque data; Filter the average torque data; When the parallel linkage joint reciprocates at a uniform speed, obtain the first angle between the end slide axis and the gravity direction of the parallel linkage joint, and the torque value when the yaw joint angle is 0°; The gravity data set is composed of multiple first angles and torque values ​​when the yaw joint angle is 0°.

[0009] Compared with the existing technology, the technical effect achieved by adopting this technical scheme is as follows: this scheme obtains a gravity data set composed of multiple first angles and torque values ​​when the yaw joint angle is 0°. On the one hand, this scheme uses the first angle between the end slide axis and the gravity direction of the parallel linkage joint as the independent variable of gravity compensation, which can directly relate the gravity torque and the posture of the end slide axis, thereby more accurately calculating the compensated gravity torque of the yaw joint at different yaw joint angles, which helps to eliminate the gravity torque error caused by the posture change of the parallel linkage joint, making gravity compensation more efficient and practical, and improving the compensation accuracy; on the other hand, the first angle can be measured in real time, so the gravity compensation method can quickly respond to changes in the posture of the parallel linkage joint, and adjust the compensated gravity torque in time, so that the movement of the robotic arm is smoother and faster.

[0010] In one embodiment of the present invention, calculating the compensating gravity torque for gravity compensation of the parallel linkage joint according to the gravity baseline dataset includes: The radial basis function network is used to learn the gravity dataset to calculate the first gravity moment of the parallel linkage joint; Based on the gravity baseline data set, the deviation value of the first gravity moment caused by the yaw compensation joint angle is obtained through the polynomial function data fitting method; The compensation gravity moment is calculated according to the first gravity moment and the deviation value.

[0011] Compared with the existing technology, the technical effects achieved by adopting this technical solution are: on the one hand, this solution calculates the first gravity moment of the parallel linkage joint through the radial basis function network, avoiding the complex dynamic modeling process and simplifying the calculation process of the compensation gravity moment; on the other hand, the polynomial function data fitting method is used to obtain the deviation value brought by the compensation yaw joint angle to the first gravity moment, and then the compensation gravity moment is calculated by the first gravity moment and the deviation value, which can accurately calculate the compensation gravity moment, making the compensation gravity moment of the torque compensation of the robotic arm more comprehensive and accurate, thereby improving the accuracy of gravity compensation.

[0012] In one embodiment of the present invention, the first angle is defined as θ, and a radial basis function network is used to learn a gravity data set to calculate the first gravity moment of the parallel linkage joint, including: Select n network nodes according to cosθ of the first angle, and obtain a time series consisting of the times output by the n network nodes and a node array consisting of the n network nodes; Obtain the actual output torque of the parallel joint motor corresponding to n network nodes; Obtaining a first end node of a first network node and a second end node of an nth network node according to n network nodes; Calculating node widths of n network nodes based on an end node distance between a first end node and a second end node; The first gravity moment is calculated according to cosθ of the first angle, n network nodes, actual output torque, and node width.

[0013] Compared with the existing technology, the technical effect achieved by adopting this technical solution is as follows: this solution calculates the first gravity moment through cosθ of the first angle, n network nodes, actual output torque, and node width, thereby further improving the accuracy of gravity compensation.

[0014] In one embodiment of the present invention, the gravity compensation method includes: According to the end node distance between the first end node and the second end node, the calculation formula for calculating the node width of n network nodes is: Formula 1: ; The calculation formula for selecting n network nodes based on the first angle cosθ is: Formula 2: ; The expression for the node array is: Formula 3: ; Where gi represents the i-th network node among n network nodes, and i=1,2,3,4...n, μ i represents a network node, σ represents the node width, and d represents the end node distance.

[0015] Compared with the existing technology, the technical effect achieved by adopting this technical solution is: further improving the accuracy of gravity compensation.

[0016] In one embodiment of the present invention, the gravity compensation method further comprises: The expression of actual output torque is: Formula 4: ; Get the node matrix consisting of n network nodes and time series, and the expression of the node matrix is: Formula 5: ; Among them, Fs1, Fs2, Fs3, ... Fsm represent the actual output force of the parallel joint motor when the first angle is at different angles, F represents the actual output torque, H represents the node matrix, n represents the network node, and m represents the time series.

[0017] In one embodiment of the present invention, the calculation formula for calculating the first gravity moment according to cosθ of the first angle, n network nodes, actual output torque, and node width is: Formula 6: ; in, fm represents the first gravity moment, Φ represents the node array, ω is the intermediate parameter, and the calculation formula of the intermediate parameter is: Formula 7: .

[0018] Compared with the existing technology, the technical effect achieved by adopting this technical solution is as follows: the weight is calculated by H and F, which further improves the real-time performance of gravity compensation.

[0019] In one embodiment of the present invention, when the end slide axis is in the first yaw posture, a second angle between the end slide axis and the parallel linkage joint in the direction of gravity is defined as α; when the end slide axis is in the second yaw posture, a third angle between the end slide axis and the parallel linkage joint in the direction of gravity is defined as β; the gravity compensation method further includes: When cosα=cosβ, obtain the first output torque of the parallel joint motor when the end slide shaft is in the first yaw posture, and the second output torque of the parallel joint motor when the end slide shaft is in the second yaw posture; A torque compensation value of the yaw compensation joint angle to the first gravity torque is obtained according to the first output torque and the second output torque.

[0020] Compared with the existing technology, the technical effect achieved by adopting this technical solution is as follows: this solution obtains the torque compensation value of the compensation yaw joint angle for the first gravity moment by comparing the output torque of the end slide axis when it is in different yaw postures and according to the first output torque and the second output torque, which can more accurately identify and compensate for the influence of the gravity torque caused by the change of the yaw joint angle, thereby more accurately analyzing the projection of the torque in the direction of gravity.

[0021] In one embodiment of the present invention, the maximum cosine value of the angle between the end slide axis and the parallel linkage joint in the direction of gravity is defined as cosδ, and the torque compensation value of the yaw compensation joint angle to the first gravity torque is obtained according to the first output torque and the second output torque, including: When cosθ=cosδ, obtain the projection value of the end slide axis on the base coordinate axis Y axis, define the projection value as X, and define the projection value when cosγ=cosδ as Y; Among them, when X>Y, Z=2(ab); when X≤Y, Z=0; Z represents the torque compensation value, a represents the torque value corresponding to cosδ of the yaw joint motor at the current yaw joint angle, b represents the actual torque value corresponding to cosθ of the current first angle, and a>b.

[0022] Compared with the existing technology, the technical effect achieved by adopting this technical solution is as follows: this solution obtains the projection value Y when cosθ=cosδ and compares the size relationship between X and Y, and calculates the torque compensation value of the first gravity moment. Specifically, when X>Y, Z=2(ab), when X≤Y, Z=0, so that the torque compensation value Z can be adjusted in real time, so that the compensated gravity torque can adapt to the rapid change of the yaw joint angle of the robot arm during movement, further improving the real-time performance of gravity compensation.

[0023] In one embodiment of the present invention, the calculation formula for calculating the compensation gravity moment according to the first gravity moment and the deviation value is: Formula 8: ; Where GP represents the compensation gravity moment, fm represents the first gravitational moment, fn Indicates the deviation value, and Z indicates the torque compensation value.

[0024] Compared with the existing technology, the technical effect achieved by adopting this technical solution is: further improving the real-time performance of gravity compensation.

[0025] After adopting the technical solution of the present invention, the following technical effects can be achieved: the present invention provides a gravity compensation method for a robotic arm, which collects the output torque data of the parallel joint motor in real time, does not require complex calculations of a large number of mechanical parameters, and avoids the complex dynamic modeling process, simplifies the calculation process of compensating gravity torque, thereby reducing the cost in the gravity compensation process and reducing time consumption, thereby shortening the response time of the robotic arm to changes in the external environment and improving the compensation effect of compensating gravity torque. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings to be used in describing the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. Those skilled in the art can also derive other drawings based on these drawings without inventive efforts. Figure 1 A schematic flow chart of a gravity compensation method for a robotic arm provided in an embodiment of the present invention; Figure 2 A schematic structural diagram of a robotic arm provided by an embodiment of the present invention; Figure 3 A graph showing the corresponding relationship between the cosθ value of the first angle and the actual output torque of a robotic arm at different yaw joint angles provided by an embodiment of the present invention; Figure 4 Schematic diagram of an RBF network provided by an embodiment of the present invention; Figure 5 A comparison chart of the actual torque data provided by the embodiment of the present invention and the output data after RBF network learning; Figure 6 The torque difference of a robotic arm at different yaw joint angles relative to a yaw joint angle of 0° provided by an embodiment of the present invention; Figure 7 for Figure 5 Taking the 30° curve as an example, the comparison chart of the compensation data curve and the actual data; Figure 8 A schematic diagram of the posture of the end slide of a robotic arm provided by an embodiment of the present invention; Figure 9 This is a corresponding relationship diagram between the actual output torque and the projection value of the end slide axis on the base coordinate axis Y axis when the yaw joint of a robotic arm is 0° provided by an embodiment of the present invention.

[0027] Description of reference numerals: 100, parallel linkage joint; 110, joint shaft; 200, end slide shaft; 300, parallel joint motor; 400, yaw joint motor; 500, base. DETAILED DESCRIPTION

[0028] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, specific embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0029] like Figure 1 As shown in FIG, a flow chart of a gravity compensation method for a robotic arm provided by the present invention is shown in FIG. Figure 2 As shown, the robot arm provided by the embodiment of the present invention includes a base 500 for supporting the main structure of the robot arm, a parallel linkage joint 100 (the parallel linkage joint 100 includes three joint shafts 110 driven by a steel belt), an end slide shaft 200 connected to the parallel linkage joint 100, and a parallel joint motor 300 and a yaw joint motor 400 for driving the parallel linkage joint 100. The gravity compensation method includes: S100: When the yaw joint motor 400 is at different yaw joint angles, the parallel linkage joint 100 is controlled to move back and forth at a uniform speed, and output torque data of the parallel joint motor 300 is collected in real time; S200: Optimizing the collected output torque data to obtain a gravity baseline data set of the parallel linkage joint 100; S300 : Calculating a compensation gravity torque for gravity compensation of the parallel linkage joint 100 according to the gravity baseline data set.

[0030] Specifically, this solution collects the output torque data of the parallel joint motor 300 in real time, does not require complex calculations of a large number of mechanical parameters, and avoids the complex dynamic modeling process, simplifies the calculation process of compensating for gravity torque, thereby reducing the cost of gravity compensation and reducing time consumption, thereby shortening the response time of the robotic arm to changes in the external environment and improving the compensation effect of compensating for gravity torque.

[0031] Furthermore, S200: optimizing the collected output torque data to obtain a gravity baseline data set of the parallel linkage joint 100, including: S210: Classify and process the output torque data; S220: Calculate the average value of the output torque data after the classification process to obtain average torque data; S230: Filtering the average torque data; S240: When the parallel linkage joint 100 reciprocates at a uniform speed, obtaining a first angle between the end slide shaft 200 and the gravity direction of the parallel linkage joint 100, and a torque value when the yaw joint angle is 0°; S250: A gravity data set is formed by the torque values ​​when the first angles and the yaw joint angle are 0°.

[0032] Specifically, this solution obtains a gravity data set composed of multiple first angles and torque values ​​when the yaw joint angle is 0°. On the one hand, since the gravity torque of the parallel joint motor 300 and the posture of the end slide shaft 200 are highly correlated, this solution uses the first angle between the end slide shaft 200 and the gravity direction of the parallel linkage joint 100 as the independent variable of gravity compensation, which can directly associate the gravity torque with the posture of the end slide shaft 200, thereby more accurately calculating the compensated gravity torque of the yaw joint at different yaw joint angles, which helps to eliminate the gravity torque error caused by the posture change of the parallel linkage joint 100, making gravity compensation more efficient and practical, and improving the compensation accuracy; on the other hand, the first angle can be measured in real time, so the gravity compensation method can quickly respond to changes in the posture of the parallel linkage joint 100, and adjust the compensated gravity torque in time, so that the movement of the robotic arm is smoother and faster.

[0033] Furthermore, based on the gravity baseline data set, a compensation gravity torque for compensating the gravity of the parallel linkage joint 100 is calculated, including: Using a radial basis function network to learn a gravity data set to calculate a first gravity moment of the parallel linkage joint 100; Based on the gravity baseline data set, the deviation value of the first gravity moment caused by the yaw compensation joint angle is obtained through the polynomial function data fitting method; The compensation gravity moment is calculated according to the first gravity moment and the deviation value.

[0034] Specifically, on the one hand, this solution calculates the first gravity moment of the parallel linkage joint 100 through a radial basis function network, avoids the complex dynamic modeling process, simplifies the calculation process of the compensation gravity moment, and thus can effectively capture the relationship between the gravity moment and the posture of the end slide axis 200, and calculate a more accurate first gravity moment; on the other hand, the polynomial function data fitting method is used to obtain the deviation value brought by the compensation yaw joint angle to the first gravity moment, and then calculates the compensation gravity moment through the first gravity moment and the deviation value, which can accurately calculate the compensation gravity moment, making the compensation gravity moment of the torque compensation of the robotic arm more comprehensive and accurate, thereby improving the accuracy of gravity compensation.

[0035] More specifically, the calculation process of the neural network model is simpler than the dynamic modeling method in the prior art, and can quickly respond to changes in the external environment, achieve real-time compensation, and improve the real-time performance of gravity compensation.

[0036] Furthermore, the first angle is defined as θ, and the radial basis function network is used to learn the gravity data set to calculate the first gravity moment of the parallel linkage joint 100, including: Select n network nodes according to cosθ of the first angle, and obtain a time series consisting of the times output by the n network nodes and a node array consisting of the n network nodes; Obtain the actual output torque of the parallel joint motor corresponding to n network nodes; Obtaining a first end node of a first network node and a second end node of an nth network node according to n network nodes; Calculating node widths of n network nodes based on an end node distance between a first end node and a second end node; The first gravity moment is calculated according to cosθ of the first angle, n network nodes, actual output torque, and node width, thereby further improving the accuracy of gravity compensation.

[0037] Furthermore, the gravity compensation method includes: According to the end node distance between the first end node and the second end node, the calculation formula for calculating the node width of n network nodes is: Formula 1: ; The calculation formula for selecting n network nodes based on the first angle cosθ is: Formula 2: ; The expression for the node array is: Formula 3: ; Where gi represents the i-th network node among n network nodes, and i=1,2,3,4...n, μ i represents the network node, σ represents the node width, and d represents the end node distance; thus, this scheme further improves the accuracy of gravity compensation.

[0038] Furthermore, the gravity compensation method further includes: The expression of actual output torque is: Formula 4: ; Get the node matrix consisting of n network nodes and time series, and the expression of the node matrix is: Formula 5: ; Among them, Fs1, Fs2, Fs3, ... Fsm represent the actual output force of the parallel joint motor 300 when the first angle is at different angles, F represents the actual output torque, H represents the node matrix, n represents the network node, and m represents the time series.

[0039] Furthermore, the calculation formula for the first gravity moment is calculated based on cosθ of the first angle, n network nodes, actual output torque, and node width: Formula 6: ; in, fm represents the first gravity moment, Φ represents the node array, ω is the intermediate parameter, and the calculation formula of the intermediate parameter is: Formula 7: .

[0040] In a specific embodiment, the collected output torque data is optimized by using step S200 of this solution, and a "cosθ-actual output torque" curve is drawn based on the processed data, such as Figure 3 As shown in FIG, the horizontal axis represents cosθ, and the vertical axis represents the actual output torque. The highest points of the curves correspond to the curves when the yaw joint is 0°, 15°, 30°, 50°, 60°, and 90°, respectively. Among them, curve W1 is 0°, curve W2 is 15°, curve W3 is 30°, curve W4 is 50°, curve W5 is 60°, and curve W6 is 90°, so as to obtain the gravity baseline data set of the parallel linkage joint 100, that is, all possible cosθ and the corresponding torque values ​​when the yaw joint is 0°, and use the radial basis function network (hereinafter referred to as RBF network) for learning. The network diagram is shown in FIG. Figure 4 As shown, Figure 5 As shown, the actual torque data is compared with the output data after RBF network learning. The curve H1 in the figure is the output data after RBF network learning, and H2 is the actual torque data. Figure 5 As can be seen from the figure, the overlap is high, indicating that the learned data can be used for gravity compensation. Ten network nodes are selected, with node centers evenly distributed between [-0.64, 1] and a node width σ = 0.5201 (the width is determined by Formula 1, where d is the end node distance and n is the number of nodes). The 10 network nodes correspond to Formula 2 above (where i = 1, 2, 3, ... 10). Furthermore, the expression for the actual output torque is: ; The node array is obtained by the above formula 3, and the node matrix composed of n network nodes and time series is obtained. The weight ω is calculated by H and F as: [-312.55, 1418.47, -3532.70, 6202.36, -8556.09, 8861.35, -7374.73, 5087.18, -2389.37, 592.04]. The first gravity moment is further calculated by the above formula 6 and output as fm, The weight is calculated by H and F, which further improves the real-time performance of gravity compensation.

[0041] Furthermore, when the end slide shaft 200 is in the first yaw posture, the second angle between the end slide shaft 200 and the parallel linkage joint 100 in the gravity direction is defined as α; when the end slide shaft 200 is in the second yaw posture, the third angle between the end slide shaft 200 and the parallel linkage joint 100 in the gravity direction is defined as β; the gravity compensation method also includes: When cosα=cosβ, obtain the first output torque of the parallel joint motor 300 when the end slide shaft 200 is in the first yaw posture, and the second output torque of the parallel joint motor 300 when the end slide shaft 200 is in the second yaw posture; A torque compensation value of the yaw compensation joint angle to the first gravity torque is obtained according to the first output torque and the second output torque.

[0042] Specifically, this solution compares the output torque of the parallel joint motor 300 when the end slide shaft 200 is in different yaw postures. Based on the first and second output torques, it obtains the torque compensation value for compensating the yaw joint angle for the first gravity moment. This more accurately identifies and compensates for the gravity torque effect caused by the yaw joint angle change, thereby more precisely analyzing the torque projection in the direction of gravity. It should be noted that cosα represents the cosine value of the second angle, and cosβ represents the cosine value of the third angle.

[0043] Furthermore, the maximum cosine value of the angle between the end slide axis and the parallel linkage joint in the gravity direction of the yaw joint motor 400 at the current yaw joint angle is defined as cosδ. Based on the first output torque and the second output torque, the torque compensation value of compensating the yaw joint angle for the first gravity torque is obtained, including: When cosθ=cosδ, obtain the projection value of the end slide axis 200 on the base coordinate axis Y axis, define the projection value as X, and define the projection value when cosγ=cosδ as Y; Among them, when X>Y, Z=2(ab); when X≤Y, Z=0; Z represents the torque compensation value, a represents the torque value of the yaw joint motor 400 corresponding to cosδ at the current yaw joint angle, b represents the actual torque value corresponding to cosθ at the current first angle, and a>b.

[0044] Compared with the existing technology, the technical effect achieved by adopting this technical solution is as follows: this solution obtains the projection value Y when cosθ=cosδ, compares the size relationship between X and Y, and calculates the torque compensation value of the first gravity moment. Specifically, when X>Y, Z=2(ab), and when X≤Y, Z=0, so that the torque compensation value Z can be adjusted in real time, so that the compensated gravity torque can adapt to the rapid change of the yaw joint angle of the robot arm during movement, further improving the real-time performance of gravity compensation.

[0045] Furthermore, when X≤Y, Z=0, that is, no torque compensation is performed, which reduces the energy consumption of the yaw joint motor and extends the service life of the yaw joint motor.

[0046] Furthermore, according to the first gravity moment and the deviation value, the calculation formula for calculating the compensation gravity moment is: Formula 8: ; Where GP represents the compensation gravity moment, fm represents the first gravitational moment, fn Indicates the deviation value, and Z indicates the torque compensation value.

[0047] In a specific embodiment of the present invention, Figure 6 As shown, the horizontal axis represents cosθ, and the vertical axis represents the actual output torque. Figure 6 The difference between different yaw joint angles and the 0° yaw joint angle is recorded. In the figure, curve Q1 is 30°, curve Q2 is 50°, curve Q3 is 60°, and curve Q4 is 90°. From the analysis of the figure, it can be found that the maximum values ​​of cosθ corresponding to different yaw joint angles are different. When cosθ approaches the minimum value of the 0° yaw joint angle (about -0.37), the minimum value of cosθ corresponding to 30° to 90° will actually exceed this value, but the corresponding data is not recorded in the figure because the difference has become constant, so it is not plotted in the figure. fn As part of the gravity compensation torque, it is output to the motor. Taking the 30° curve as an example, Figure 7 As shown in the figure, curve E1 is the compensation data curve, and curve F1 is the actual data curve. The comparison between the compensation data curve and the actual data curve is plotted. It can be seen from the figure that the deviation value caused by the compensation yaw joint angle to the first gravity moment is obtained by the polynomial function data fitting method. After compensating the first gravity moment, the deviation between the compensation data and the actual data is reduced. It should be noted that Figure 7 In the figure, the horizontal axis represents cosθ and the vertical axis represents the actual output torque.

[0048] Furthermore, when the yaw joint angle is between 0° and 60°, the end slide posture will appear Figure 8 The situation shown in : that is, the cosθ corresponding to the two postures is the same, but the posture 1 (such as Figure 8 The gravity of the middle straight line S1 requires the joint motor to output a clockwise torque to overcome the posture 2 (such as Figure 8The gravity of the straight line S2) requires the joint motor to output torque counterclockwise. The torques corresponding to the two postures are equal in magnitude and opposite in direction. Therefore, a new variable is needed to identify the corresponding posture and output the appropriate gravity compensation torque according to the posture. This variable is the torque compensation value. Taking the case of yaw 0° as an example, when cosα is at its maximum value, any first projection value point and second projection value point with the same difference from the maximum value are taken, and the first torque value of the first projection value point and the second torque value of the second projection value point are obtained (such as Figure 9 ), Figure 9 The middle curve E2 is the actual output torque curve. The horizontal coordinate of the curve E2 represents cosθ and the vertical coordinate represents the actual output torque. Figure 9 The middle curve E2 is the projection curve (ZY) of the end slide axis on the gravity axis, where the abscissa of the curve E2 represents cosθ and the ordinate represents the projection value of the end slide axis on the base coordinate axis Y. The torque values ​​corresponding to points C and D are the same as the difference between the torque values ​​corresponding to point B. When X>Y, Z=2(ab); when X≤Y, Z=0. This formula is Figure 9 In the figure, Y represents the vertical coordinate of point A, a represents the vertical coordinate of point B, and b represents the vertical coordinate of point C. Finally, the compensation gravity moment is calculated according to the above formula 8, which further improves the real-time performance of gravity compensation.

[0049] Although the present invention is disclosed as above, the present invention is not limited thereto. Any person skilled in the art can make various changes and modifications without departing from the spirit and scope of the present invention. Therefore, the scope of protection of the present invention should be based on the scope defined by the claims.

Claims

1. A gravity compensation method for a robotic arm, characterized in that: The robotic arm comprises a parallel linkage joint (100), an end slide shaft (200) connected to the parallel linkage joint (100), and a parallel joint motor (300) and a yaw joint motor (400) for driving the parallel linkage joint (100). The gravity compensation method comprises: When the yaw joint motor (400) is at different yaw joint angles, controlling the parallel linkage joint (100) to reciprocate and move at a uniform speed, and collecting output torque data of the parallel joint motor (300) in real time; Optimizing the collected output torque data to obtain a gravity baseline data set of the parallel linkage joint (100); A compensation gravity torque for compensating the gravity of the parallel linkage joint (100) is calculated based on the gravity baseline data set.

2. The gravity compensation method according to claim 1, characterized in that: The step of optimizing the collected output torque data to obtain a gravity baseline data set of the parallel linkage joint (100) includes: performing classification processing on the output torque data; calculating an average value of the output torque data after the classification process to obtain average torque data; performing filtering processing on the average torque data; When the parallel linkage joint (100) reciprocates at a uniform speed, obtaining a first angle between the end slide shaft (200) and the gravity direction of the parallel linkage joint (100), and a torque value when the yaw joint angle is 0°; The gravity data set is composed of a plurality of first angles and torque values ​​when the yaw joint angle is 0°.

3. The gravity compensation method according to claim 2, characterized in that: Calculating the compensation gravity torque for gravity compensation of the parallel linkage joint (100) based on the gravity baseline data set includes: Using a radial basis function network to learn the gravity data set to calculate the first gravity moment of the parallel linkage joint (100); Based on the gravity baseline data set, a polynomial function data fitting method is used to obtain a deviation value of compensating for the first gravity moment caused by the yaw joint angle; The compensation gravity moment is calculated according to the first gravity moment and the deviation value.

4. The gravity compensation method according to claim 3, characterized in that: The first angle is defined as θ, and the radial basis function network is used to learn the gravity data set to calculate the first gravity moment of the parallel linkage joint (100), including: Selecting n network nodes according to cosθ of the first angle, and obtaining a time series consisting of times output by the n network nodes and a node array consisting of the n network nodes; Obtaining the actual output torque of the parallel joint motor corresponding to the n network nodes; Acquire a first end node of a first network node and a second end node of an nth network node according to the n network nodes; Calculating a node width of the n network nodes according to an end node distance between the first end node and the second end node; The first gravity moment is calculated according to cosθ of the first angle, the n network nodes, the actual output torque, and the node width.

5. The gravity compensation method according to claim 4, characterized in that: The gravity compensation method comprises: The calculation formula for calculating the node width of the n network nodes based on the end node distance between the first end node and the second end node is: Formula 1: ; The calculation formula for selecting n network nodes according to cosθ of the first angle is: Formula 2: ; The expression of the node array is: Formula 3: ; Wherein, gi represents the i-th network node among the n network nodes, and i=1, 2, 3, 4...n, μi represents the network node, σ represents the node width, and d represents the end node distance.

6. The gravity compensation method according to claim 5, characterized in that: The gravity compensation method further comprises: The expression of the actual output torque is: Formula 4: ; Obtain a node matrix consisting of the n network nodes and the time series, and the expression of the node matrix is: Formula 5: ; Wherein, Fs1, Fs2, Fs3, ... Fsm represent the actual output force of the parallel joint motor (300) when the first angle is at different angles, F represents the actual output torque, H represents the node matrix, n represents the network node, and m represents the time series.

7. The gravity compensation method according to claim 6, characterized in that: The calculation formula for calculating the first gravity moment according to cosθ of the first angle, the n network nodes, the actual output torque, and the node width is: Formula 6: ; in, fm represents the first gravity moment, Φ represents the node array, ω is an intermediate parameter, and the calculation formula of the intermediate parameter is: Formula 7: .

8. The gravity compensation method according to claim 4, characterized in that: When the end slide shaft (200) is in a first yaw posture, a second angle between the end slide shaft (200) and the parallel linkage joint (100) in the direction of gravity is defined as α; when the end slide shaft (200) is in a second yaw posture, a third angle between the end slide shaft (200) and the parallel linkage joint (100) in the direction of gravity is defined as β; The gravity compensation method further comprises: When cosα=cosβ, obtaining a first output torque of the parallel joint motor (300) when the end slide shaft (200) is in the first yaw posture, and a second output torque of the parallel joint motor (300) when the end slide shaft (200) is in the second yaw posture; A torque compensation value for compensating the yaw joint angle for the first gravity moment is obtained according to the first output torque and the second output torque.

9. The gravity compensation method according to claim 7, characterized in that: Defining the maximum cosine value of the angle between the end slide shaft (200) and the parallel linkage joint in the direction of gravity as cosδ, and obtaining the torque compensation value for compensating the yaw joint angle for the first gravity torque based on the first output torque and the second output torque, including: When cosθ=cosδ, obtaining the projection value of the end slide axis (200) on the base coordinate axis Y axis, defining the projection value as X, and defining the projection value when cosθ=cosδ as Y; Among them, when X>Y, Z=2(ab); when X≤Y, Z=0; Z represents the torque compensation value, a represents the torque value of the yaw joint motor (400) corresponding to cosδ at the current yaw joint angle, b represents the actual torque value corresponding to cosθ at the current first angle, and a>b.

10. The gravity compensation method according to claim 9, characterized in that: The calculation formula for calculating the compensation gravity moment according to the first gravity moment and the deviation value is: Formula 8: ; Wherein, GP represents the compensating gravity moment, fm represents the first gravitational moment, fn represents the deviation value, and Z represents the torque compensation value.

Images