Damping control method and system for linkage type hydraulic mechanical arm

Abstract

The invention relates to the technical field of automatic control, and discloses a damping control method and system for a linkage type hydraulic mechanical arm to improve the overall performance. The method comprises the steps that given constraint parameters of a target hydraulic cylinder are obtained; at least one re-planning time is set before the target displacement is reached, and after the re-planning time arrives, the initial speed distribution curve of the remaining displacement is re-calculated according to the constraint parameters, the current actual displacement of the target hydraulic cylinder and the theoretical values of the current acceleration and the speed calculated based on the speed distribution curve of the last re-planning; the maximum acceleration time, the maximum constant speed time and the maximum deceleration time obtained through statistics after the initial speed distribution curve of the target hydraulic cylinder and the initial speed distribution curves of the other linked hydraulic cylinders are summarized are set as the common acceleration time, constant speed time and deceleration time of the linked hydraulic cylinders; and calculating a re-planned speed distribution curve of the target hydraulic cylinder according to the common acceleration time, constant speed time and deceleration time.

Description

Technical Field

[0001] The present invention relates to the field of automatic control technology, and in particular to a shock absorption control method and system for a linkage type hydraulic mechanical arm. Background Art

[0002] Hydraulic manipulators are widely used in the field of engineering machinery due to their advantages such as high load, high reliability and easy maintenance. In the control of hydraulic manipulators, PID control algorithms and fuzzy control algorithms are generally used. However, since the hydraulic cylinder exhibits strong nonlinear characteristics in the control, it poses a great challenge to the adjustment of the control algorithm.

[0003] In terms of speed planning of hydraulic manipulators, speed planning algorithms can effectively reduce the difficulty of control parameter adjustment. Common speed planning algorithms mainly include linear interpolation and trapezoidal speed curves, but there are still many shortcomings. Thread interpolation does not take into account the continuity of speed and acceleration, which will cause the hydraulic manipulator to produce large vibrations during control. The trapezoidal speed curve has improved in alleviating vibrations due to its speed continuity, but because the acceleration at the intersection of the trapezoid is discontinuous, it cannot perfectly solve this problem. The use of polynomial interpolation can solve the problem of continuity of speed, acceleration and jerk. Generally, a cubic polynomial or a quintic polynomial is used to obtain the speed curve, thereby obtaining a smooth and reasonable speed curve. However, the above speed planning algorithm is mainly aimed at the single-joint movement of the hydraulic manipulator. For the multi-joint movement of the hydraulic manipulator, especially in the process of unmanned operation and maintenance of the hydraulic manipulator, the hydraulic manipulator needs to be able to actively detect surrounding obstacles and avoid them. This puts higher requirements on speed planning, that is, when the hydraulic manipulator finds an obstacle and needs multi-joint reverse movement, the manipulator needs to stop slowly and move in the opposite direction. No obvious vibration should be generated in this process. This requires the speed and acceleration planned by the controller to ensure its continuity, and multiple joints need to arrive at the same time, so as to ensure that the moving trajectory of the end of the manipulator and the target trajectory are consistent to realize the autonomous obstacle avoidance function. In particular, the hydraulic manipulator is a control object with low precision. After a certain period of operation, if the speed is not re-planned during the movement, the control error is not compensated or the control error is cleared, after the manipulator reaches the end point, there will be a large error between its actual displacement and the target displacement. How to control and reduce the error between the actual displacement and the target displacement of the hydraulic manipulator poses a challenge to the existing control methods. Summary of the invention

[0004] The present invention aims to disclose a shock absorption control method and system for a linkage type hydraulic mechanical arm to improve the overall performance.

[0005] To achieve the above-mentioned purpose, the shock absorption control method of the linkage hydraulic mechanical arm disclosed in the present invention comprises: Step S1, obtaining constraint parameters given by the target hydraulic cylinder, wherein the constraint parameters include the maximum acceleration in the acceleration section, the speed in the uniform speed section, the maximum deceleration in the deceleration section, the target displacement, and the terminal speed when reaching the target displacement; Step S2, setting at least one replanning time before reaching the target displacement, and after the replanning time arrives, recalculating the initial velocity distribution curve of the remaining displacement according to the constraint parameters, the current actual displacement of the target hydraulic cylinder, and the theoretical values ​​of the current acceleration and velocity calculated based on the velocity distribution curve of the last replanning; Step S3, the maximum acceleration time, maximum uniform speed time and maximum deceleration time obtained by summarizing the initial speed distribution curve of the target hydraulic cylinder and the initial speed distribution curves of other linked hydraulic cylinders are set as the common acceleration time, uniform speed time and deceleration time of each linked hydraulic cylinder; Step S4, calculating the re-planned speed distribution curve of the target hydraulic cylinder according to the constraint parameters, the current actual displacement of the target hydraulic cylinder, the theoretical values ​​of the current acceleration and speed calculated based on the speed distribution curve of the previous re-planning, and the common acceleration time, uniform speed time and deceleration time.

[0006] Preferably, the next re-planning time is the product of the total time accumulated from the current common acceleration time, uniform speed time and deceleration time and the set proportional coefficient.

[0007] Preferably, the value of the proportionality coefficient is greater than 0.2 and less than 0.5.

[0008] Preferably, in step S4, if the common acceleration time and the uniform speed time are both 0, a quintic polynomial is used to solve the re-planned speed distribution curve; otherwise, a quartic polynomial is used to solve the re-planned speed distribution curve.

[0009] Preferably, in step S2, the process of recalculating the initial velocity distribution curve of the residual displacement includes: First, the acceleration time and deceleration time are calculated according to the uniform speed in the given constraint parameters combined with the current acceleration and speed of the target hydraulic cylinder, and then it is determined whether the sum of the displacement corresponding to the acceleration time and the displacement corresponding to the deceleration time is greater than the remaining displacement. If so, the speed of the inflection point where the acceleration stage is directly switched to the deceleration stage is calculated according to the maximum acceleration of the acceleration stage, the maximum deceleration of the deceleration stage, the terminal speed when the target displacement is reached, and the remaining displacement in the constraint conditions, and then the acceleration time and deceleration time corresponding to the initial speed distribution curve are recalculated according to the speed of the inflection point; otherwise, the uniform time corresponding to the uniform speed of the initial speed distribution curve in the given constraint parameters is calculated.

[0010] To achieve the above-mentioned purpose, the present invention also discloses a shock absorption control system for a linked hydraulic mechanical arm, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the above-mentioned method when executing the computer program.

[0011] The present invention has the following beneficial effects: 1. In step S2 and step S4, the initial and re-planned velocity distribution curves of the residual displacement are recalculated based on the theoretical values ​​of the current acceleration and velocity calculated by the velocity distribution curve of the last re-planning, which can ensure that the speed and acceleration of the two adjacent re-planning switching connections are completely continuous, thereby achieving a shock absorption effect; at the same time, the residual displacement is calculated based on the measured displacement and then the initial velocity distribution curve is calculated, taking into account the accuracy of the mobile displacement execution.

[0012] 2. In the re-planned speed distribution curve, the linked hydraulic cylinders are re-planned with a common acceleration time, uniform speed time and deceleration time, ensuring the synchronization of the actions performed by the joints of the robotic arm and the consistency of the targets, thereby achieving simultaneous arrival of multiple joints.

[0013] The present invention will be further described in detail below with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] The drawings constituting a part of this application are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings: Figure 1 It is a flow chart of a shock absorption control method for a linkage type hydraulic mechanical arm disclosed in an embodiment of the present invention. DETAILED DESCRIPTION

[0015] The embodiments of the present invention are described in detail below with reference to the accompanying drawings, but the present invention can be implemented in many different ways as defined and covered by the claims.

[0016] Example 1 In this embodiment, each speed planning may generate three common speed times for the linked hydraulic cylinders: acceleration time, constant speed time, and deceleration time. The total time of a single planning is obtained by summing up the above three times. ; where the subscript j represents the number of replanning. Inside the controller, when the timer is equal to 0.25× , we can use the last (i.e. j-1th) control fragment to control the The speed and acceleration theoretical values ​​are used as the planning starting point for re-planning. At this time, the internal timer will be reset to zero, and the waiting timer will be equal to 0.25× Continue to replan until the hydraulic cylinder is located around the target displacement (a relatively small deviation), and the hydraulic cylinder completes speed control.

[0017] Set 0.25× The main reasons for the boundary conditions are: The replanning boundary is related to the total time. The longer the total time, the longer the replanning interval. This is in line with the actual planning boundary. Because when the target displacement is closer, the total time accumulated by the replanning acceleration time, uniform speed time and deceleration time is longer. The shorter the speed, the more frequently speed replanning is performed to compensate for control errors.

[0018] When the displacement is far from the target, the boundary condition interval is long, so there is no need for frequent re-planning, which also reduces the computational burden of the controller.

[0019] In order to clearly explain the core of this embodiment, the following auxiliary contents are first explained in sections: 1. Displacement time planning The initial and final accelerations in the deceleration and acceleration phases are set to 0, which does not meet engineering needs. Therefore, after calculating the acceleration, constant speed, and deceleration time, we need to re-plan the speed according to the actual state of the robot.

[0020] Assume that the acceleration time expression of a single hydraulic cylinder of the robotic arm is as follows: (1) In the formula, according to the actual project trial and error , . is the maximum acceleration in the acceleration stage, It is the maximum deceleration in the deceleration stage. , , are the parameters to be determined, representing the acceleration section, deceleration section and the duration of the entire speed section. = , = - .

[0021] Based on common sense of technicians in this field, It can be approximated as the following formula: (2) Simplifying formula (1) we can get: (3) In the formula, .

[0022] Integrate formula (3) to obtain the speed curve: (4) In the formula, is the initial velocity in the acceleration stage, is a given uniform speed.

[0023] In contrast, the existing technical route for the above formula Generally, the actual speed of the current hydraulic cylinder obtained by the sensor will cause sensor interference and jitter to the algorithm. It is the target speed in the control segment of the previous frame. Its benefits are: 1. Avoids sensor interference and increases the robustness of the algorithm.

[0024] 2. The starting point of this planning is the end point of the previous planning, which ensures that the speed curve and acceleration curve after re-planning remain continuous during the entire life cycle of the hydraulic cylinder control, and has a more obvious improvement on the jitter phenomenon caused by re-planning in the hydraulic cylinder motion control.

[0025] According to formula (4) and the given deceleration stage end speed , the duration of the acceleration and deceleration phases can be obtained as: (5) Integrate equation (4) and substitute equation (5) to obtain the expression of displacement over time: (6) In the formula, is the displacement in the acceleration phase, is the displacement in the uniform velocity stage. = ; = - ; = - - ; In the formula, is the displacement during the deceleration phase.

[0026] 2. Determine whether to re-plan the time based on the displacement Assume that the displacement of the entire hydraulic cylinder is .when + < When , there is a uniform speed period. =( - - ) / .

[0027] If it appears + > , it means that there is no uniform speed stage in this plan, so it is necessary to recalculate the acceleration and deceleration time. At this time, the entire motion process has only two processes: deceleration and acceleration, that is, =0. Therefore, we can get: (7) , are the displacements corresponding to the acceleration stage and deceleration stage after redistribution, In the first planning, it is the target displacement, and in the subsequent iterations, it is the residual displacement obtained by the difference between the target displacement and the actual displacement currently completed.

[0028] The formula (6) can be modified to obtain: (8) in, = , = + .

[0029] From formula (8), the duration of acceleration and deceleration after replanning is: (9) Combining equations (7~9), we can get for: (10) Through formula (10), we can calculate , .

[0030] Based on the above derivation, the time required for a single hydraulic cylinder in the acceleration section, the constant speed section, and the deceleration section can be calculated. It is worth noting that in this embodiment, the hydraulic cylinder is controlled at the maximum acceleration and the maximum deceleration during the movement, which will not be described in detail later. At the same time, in each replanning process, based on the above derivation, the velocity distribution curve of a single hydraulic cylinder based on the initial remaining target can be calculated; and in the subsequent replanning process, the above , and All of them need to be recalculated according to formula (5) or formula (9) in order to accurately execute the subsequent goals - the initial speed distribution curves of each hydraulic cylinder in linkage and the maximum acceleration time, maximum uniform speed time and maximum deceleration time obtained after summarization are set as the common acceleration time, uniform speed time and deceleration time; and then the re-planned speed distribution curves of each hydraulic cylinder are determined according to the aforementioned common time.

[0031] 3. Let the common acceleration time be , the uniform speed time is recorded as , the deceleration time is recorded as , then the speed curve for replanning has the following two situations: First case: (acceleration phase time) and (Deceleration phase time) are all greater than 0 3.1.1. Assume that the target displacement of a hydraulic cylinder is The initial speed of the acceleration stage is (the target velocity of the last controlled segment), and the acceleration is (the target acceleration of the last control segment), and the terminal velocity is (to be determined), the terminal acceleration is 0 (transition from the acceleration stage to the uniform speed stage, so the acceleration is 0 at this time); the initial velocity of the deceleration stage is (that is, the speed in the uniform speed stage, which is the quantity to be determined), the acceleration is 0 (from the uniform speed stage to the deceleration stage, so the acceleration is 0 at this time), and the terminal speed is (given at the planning layer), the end acceleration is 0 (the end point of the entire planning, in order to ensure the smooth transition of the robot arm between points).

[0032] Setting the acceleration and deceleration stage velocity curves to a quartic polynomial, the displacement, velocity, and acceleration curves are as follows (the acceleration and deceleration stage curves can be expressed by the following formula): (11) According to the above formula, we can get the following equation: =0; that is, the initial displacement in the acceleration phase is 0.

[0033] = ; That is, the initial speed of the acceleration phase is .

[0034] = ; That is, the initial acceleration in the acceleration phase is .

[0035] = ; That is (the terminal speed of the acceleration phase is .

[0036] =0; that is, the terminal acceleration of the acceleration phase is 0.

[0037] Expand the above formulas one by one, and we get the following: (12) Further deducing and solving the above formula, we can get the coefficient expression of the quartic polynomial as follows: (13) Next, we can determine the total displacement during the acceleration phase The expression is as follows: (14) 3.1.2. Solve the speed curve in the deceleration stage: When there is a uniform speed stage, the initial speed of the acceleration in the deceleration stage is the acceleration and speed in the uniform speed stage; when there is no uniform speed stage, the acceleration and initial speed in the deceleration stage are the acceleration and speed at the end of the acceleration stage. The acceleration and speed in the uniform speed stage are the acceleration and speed at the end of the acceleration stage. Therefore, the initial acceleration in the deceleration stage is 0, and the initial speed is ; The initial displacement is 0; The terminal velocity is , the terminal acceleration is 0. Therefore, the following equation can be obtained: =0; that is, the initial displacement of the deceleration stage is 0, where = + + .

[0038] = ; That is, the initial speed of the deceleration stage is .

[0039] =0; that is, the initial acceleration of the deceleration stage is 0.

[0040] = ; That is, the terminal speed of the deceleration stage is .

[0041] =0; that is, the terminal acceleration of the deceleration stage is 0.

[0042] Expand the above formulas one by one, and we get the following: (15) Further deducing and solving the above formula, we can get the coefficient expression of the quartic polynomial in the deceleration stage as follows: (16) Next, we can determine the total displacement during the deceleration phase The expression is as follows: (17) The total displacement in the uniform velocity stage is easily obtained = * .

[0043] Therefore, the total displacement It can be expressed as follows: (18) By modifying the total displacement, we can get: (19) The above formula, =0 (i.e., only the acceleration phase and the deceleration phase are planned).

[0044] Will Substituting the acceleration and deceleration stages into the speed curves of the acceleration and deceleration stages can be obtained.

[0045] Second case: (Acceleration phase time) is 0, (Deceleration phase time) greater than 0 If there is only the deceleration phase time, but no acceleration phase and uniform speed phase time, it can be found that the initial velocity of the deceleration phase is , is not equal to the expected speed of the previous control cycle (i.e. the initial speed of the current control cycle), which leads to a serious problem that the speed curve is no longer continuous, which will cause control-induced vibration to the hydraulic cylinder. Therefore, when the planning result only has the deceleration stage time, this embodiment needs to use a quintic polynomial to solve to ensure the speed continuity after re-planning.

[0046] Setting the speed curve in the deceleration stage to a quintic polynomial, the displacement, velocity, and acceleration curves are as follows: (20) Analysis shows that the initial speed in the deceleration stage is , the initial acceleration is The terminal speed is , the terminal acceleration is 0; the initial displacement is 0, and the terminal displacement is the target displacement .

[0047] According to the above analysis, the following equation can be obtained: =0; that is, the initial displacement of the deceleration stage is 0.

[0048] = ; That is, the end displacement of the deceleration stage is .

[0049] = ; That is, the initial speed of the deceleration stage is .

[0050] = ; That is, the initial acceleration in the deceleration phase is .

[0051] = ; That is, the terminal speed of the deceleration stage is .

[0052] =0; that is, the terminal acceleration of the deceleration stage is 0.

[0053] Expand the above formulas one by one, and we get the following: (twenty one) Further deducing and solving the above formula, we can get the coefficient expression of the fifth-order polynomial in the deceleration stage as follows: (twenty two) Based on the above content, the shock absorption control method of the linkage hydraulic mechanical arm disclosed in this embodiment includes: Step S1, obtaining constraint parameters given by the target hydraulic cylinder, wherein the constraint parameters include the maximum acceleration in the acceleration section, the speed in the uniform speed section, the maximum deceleration in the deceleration section, the target displacement, and the terminal speed when reaching the target displacement.

[0054] In this step, the maximum acceleration of the acceleration section is , the uniform speed is , the maximum deceleration in the deceleration section is , the target displacement is , the terminal velocity when reaching the target displacement is .

[0055] Step S2, setting at least one replanning time before reaching the target displacement, and after the replanning time arrives, recalculating the initial velocity distribution curve of the remaining displacement according to the constraint parameters, the current actual displacement of the target hydraulic cylinder and the theoretical values ​​of the current acceleration and velocity calculated based on the velocity distribution curve of the last replanning.

[0056] In this step, refer to the auxiliary content corresponding to sequence number 2 in the above embodiment; the process of recalculating the initial velocity distribution curve of the residual displacement includes: firstly calculating the acceleration time and the deceleration time according to the uniform speed in the given constraint parameters combined with the current acceleration and speed of the target hydraulic cylinder (that is, obtaining the duration of the acceleration and deceleration stages according to Formula 5), ​​and then determining whether the sum of the displacement corresponding to the acceleration time and the displacement corresponding to the deceleration time is greater than the residual displacement, if so, calculating the speed of the inflection point where the acceleration stage is directly switched to the deceleration stage according to the maximum acceleration of the acceleration stage, the maximum deceleration of the deceleration stage, the terminal speed when reaching the target displacement and the residual displacement in the constraint conditions (that is, Formula 10), and then recalculating the acceleration time and deceleration time corresponding to the initial velocity distribution curve according to the speed of the inflection point (that is, Formula 9); otherwise, calculating the uniform time corresponding to the uniform speed of the initial velocity distribution curve in the given constraint parameters.

[0057] Step S3, summarizing the initial speed distribution curve of the target hydraulic cylinder and the initial speed distribution curves of other linked hydraulic cylinders, and setting the maximum acceleration time, maximum uniform speed time and maximum deceleration time obtained by statistics as the common acceleration time, uniform speed time and deceleration time of each linked hydraulic cylinder.

[0058] Step S4, calculating the re-planned speed distribution curve of the target hydraulic cylinder according to the constraint parameters, the current actual displacement of the target hydraulic cylinder, the theoretical values ​​of the current acceleration and speed calculated based on the speed distribution curve of the previous re-planning, and the common acceleration time, uniform speed time and deceleration time.

[0059] In step S4, if the common acceleration time and the uniform speed time are both 0, the quintic polynomial corresponding to the second case in the above auxiliary content three is used to solve the re-planned speed distribution curve; otherwise, the quartic polynomial corresponding to the first case in the above auxiliary content three is used to solve the re-planned speed distribution curve.

[0060] Preferably, the next re-planning time is the product of the total time after the current common acceleration time, uniform speed time and deceleration time are accumulated and the set proportional coefficient. The value of the proportional coefficient is greater than 0.2 and less than 0.5; for example, 0.25 as mentioned above.

[0061] Example 2 This embodiment discloses a shock absorption control system for a linkage hydraulic mechanical arm, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the above method when executing the computer program.

[0062] In summary, the methods and systems disclosed in the above embodiments of the present invention respectively have the following beneficial effects: 1. In step S2 and step S4, the initial and re-planned velocity distribution curves of the residual displacement are recalculated based on the theoretical values ​​of the current acceleration and velocity calculated by the velocity distribution curve of the last re-planning, which can ensure that the speed and acceleration of the two adjacent re-planning switching connections are completely continuous, thereby achieving a shock absorption effect; at the same time, the residual displacement is calculated based on the measured displacement and then the initial velocity distribution curve is calculated, taking into account the accuracy of the mobile displacement execution.

[0063] 2. In the re-planned speed distribution curve, the linked hydraulic cylinders are re-planned with a common acceleration time, uniform speed time and deceleration time, ensuring the synchronization of the actions performed by the joints of the robotic arm and the consistency of the targets, thereby achieving simultaneous arrival of multiple joints.

[0064] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A method for controlling shock absorption of a linkage hydraulic mechanical arm, characterized in that: include: Step S1, obtaining constraint parameters given by the target hydraulic cylinder, wherein the constraint parameters include the maximum acceleration in the acceleration section, the speed in the uniform speed section, the maximum deceleration in the deceleration section, the target displacement, and the terminal speed when reaching the target displacement; Step S2, setting at least one replanning time before reaching the target displacement, and after the replanning time arrives, recalculating the initial velocity distribution curve of the remaining displacement according to the constraint parameters, the current actual displacement of the target hydraulic cylinder, and the theoretical values ​​of the current acceleration and velocity calculated based on the velocity distribution curve of the last replanning; Step S3, the maximum acceleration time, maximum uniform speed time and maximum deceleration time obtained by summarizing the initial speed distribution curve of the target hydraulic cylinder and the initial speed distribution curves of other linked hydraulic cylinders are set as the common acceleration time, uniform speed time and deceleration time of each linked hydraulic cylinder; Step S4, calculating the re-planned speed distribution curve of the target hydraulic cylinder according to the constraint parameters, the current actual displacement of the target hydraulic cylinder, the theoretical values ​​of the current acceleration and speed calculated based on the speed distribution curve of the previous re-planning, and the common acceleration time, uniform speed time and deceleration time.

2. The method according to claim 1, characterized in that The next planning time is the product of the total time after the current common acceleration time, uniform speed time and deceleration time are accumulated and the set proportional coefficient.

3. The method according to claim 2, characterized in that The value of the proportionality coefficient is greater than 0.2 and less than 0.

5.

4. The method according to any one of claims 1 to 3, characterized in that: In step S4, if the common acceleration time and the uniform speed time are both 0, a quintic polynomial is used to solve the re-planned speed distribution curve; otherwise, a quartic polynomial is used to solve the re-planned speed distribution curve.

5. The method according to claim 4, characterized in that In step S2, the process of recalculating the initial velocity distribution curve of the residual displacement includes: First, the acceleration time and deceleration time are calculated according to the uniform speed in the given constraint parameters combined with the current acceleration and speed of the target hydraulic cylinder, and then it is determined whether the sum of the displacement corresponding to the acceleration time and the displacement corresponding to the deceleration time is greater than the remaining displacement. If so, the speed of the inflection point where the acceleration stage is directly switched to the deceleration stage is calculated according to the maximum acceleration of the acceleration stage, the maximum deceleration of the deceleration stage, the terminal speed when the target displacement is reached, and the remaining displacement in the constraint conditions, and then the acceleration time and deceleration time corresponding to the initial speed distribution curve are recalculated according to the speed of the inflection point; otherwise, the uniform time corresponding to the uniform speed of the initial speed distribution curve in the given constraint parameters is calculated.

6. A shock absorption control system for a linkage hydraulic mechanical arm, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the method according to any one of claims 1 to 5 is implemented.

Images