An algorithm for optimizing the relative position control of robotic arm motors

Abstract

The present invention belongs to the technical field of robotic arm motors, and particularly relates to an algorithm for optimizing the relative position control of robotic arm motors, including the first step: expanding the state observer, and then performing disturbance estimation and acceleration estimation through the extended state observer, so as to serve as the preliminary work for the active disturbance rejection device and the online inertia estimation; the second step: determining whether the online inertia estimation in the first step meets the condition of the minimum acceleration; the third step: online estimating the inertia; the fourth step: updating the controller parameters in real time according to the inertia estimated in the third step to improve the system's adaptive ability; the fifth step: using the disturbance estimation value for active disturbance rejection and cooperating with the fourth step for disturbance compensation to improve the system's real-time response ability. The present invention can solve the problems existing in the existing relative position control algorithm of robotic arm motors, such as slow response speed, poor adaptive ability, and high requirements for the accuracy of the system model.

Description

Technical Field

[0001] The present invention belongs to the technical field of robotic arm motors, and particularly relates to an algorithm for optimizing the relative position control of robotic arm motors. Background Art

[0002] In the fields of industrial automation and precision operations, robotic arms are increasingly widely used. Especially in environments such as medical surgeries, precision assembly, and automated production lines, extremely high requirements are imposed on the control accuracy and response speed of robotic arms. As one of the core control strategies, the relative position control of robotic arm motors directly determines the operation accuracy and real-time performance of robotic arms.

[0003] However, the existing relative position control schemes face many challenges in practical applications. First, due to the unknown nature of the motion trajectory in relative position control, traditional feedforward compensation methods based on known trajectories are difficult to directly apply, which limits the improvement of system response performance. In scenarios such as medical surgeries that require high real-time response, the robotic arm must be able to quickly and accurately track the doctor's operation actions, but the existing control algorithms often fail to meet this requirement.

[0004] Secondly, the existing control schemes are insufficient in terms of adaptability. For example, when the robotic arm grasps a heavy object and causes a change in the system inertia, the controller often cannot perceive and adjust the control parameters in real time, resulting in a decline in control performance. This sensitivity to system parameter changes limits the application of robotic arms in complex and variable environments.

[0005] In addition, the existing control algorithms have high requirements for the accuracy of the system model, and there are often various unknown disturbances and model errors in the actual system. The observer designed based on the model is prone to large errors when estimating the disturbance amount, which in turn affects the performance of the controller.

[0006] In view of the above disadvantages, the present invention proposes an algorithm for optimizing the relative position control of robotic arm motors to solve the above problems. Summary of the Invention

[0007] The object of the present invention is to provide an algorithm for optimizing the relative position control of robotic arm motors, which can solve the problems existing in the existing relative position control algorithms of robotic arm motors, such as slow response speed, poor adaptability, and high requirements for the accuracy of the system model, and provide a more efficient and reliable control solution for the fields of industrial automation and precision operations.

[0008] The technical solution adopted by the present invention is specifically as follows:

[0009] An algorithm for optimizing the relative position control of robotic arm motors includes the following steps:

[0010] Step 1: Expand the state observer, and then perform disturbance estimation and acceleration estimation through the extended state observer, which serves as the preliminary work for the active disturbance rejection device and the online inertia estimation.

[0011] Step 2: Determine whether the online inertia estimation in S1 meets the condition of the minimum acceleration.

[0012] Step 3: Estimate the inertia online.

[0013] Step 4: Update the controller parameters in real time according to the estimated inertia in S3 to improve the system's adaptive ability.

[0014] Step 5: Use the disturbance estimation value for active disturbance rejection and cooperate with S4 for disturbance compensation to improve the system's real-time response ability.

[0015] The technical effects achieved by the present invention are as follows:

[0016] The present invention can improve the response speed and adaptive ability of the control system, and at the same time reduce the dependence on the accuracy of the system model. Specifically, the present invention introduces an active disturbance rejection algorithm to make up for the deficiencies of feedforward control in relative position control, effectively improving the response performance of the control system;

[0017] At the same time, the present invention also proposes a set of inertia estimation algorithms and logics, which can dynamically optimize the controller parameters in real time according to the estimated inertia, thereby enhancing the adaptive ability of the system;

[0018] In addition, the present invention also improves the traditional observer by using the method of the extended state observer (LESO), reduces the dependence on the accurate system model, and significantly improves the control accuracy and robustness of the system by configuring the controller and observer bandwidth;

[0019] In summary, the present invention aims to solve the problems existing in the relative position control algorithm of the existing robotic arm motor, such as slow response speed, poor adaptive ability, and high requirements for the accuracy of the system model, and provides a more efficient and reliable control solution for the fields of industrial automation and precision operation. Description of the Drawings

[0020] Figure 1 is the overall flowchart of the present invention;

[0021] Figure 2 is the processing flowchart of disturbance estimation in the present invention;

[0022] Figure 3 is the specific flowchart of acceleration estimation in the present invention;

[0023] Figure 4 is the calculation flowchart of the disturbance force estimation value in the present invention;

[0024] Figure 5 is the conversion flowchart of online inertia estimation in the present invention;

[0025] Figure 6 is the operation flowchart of the state machine in the present invention;

[0026] Figure 7 is the control block diagram of the PI controller in the present invention;

[0027] Figure 8 is the specific flowchart of active disturbance rejection in the present invention. Specific Embodiments

[0028] In order to make the objectives and advantages of the present invention clearer and more understandable, the present invention will be specifically described below in conjunction with embodiments. It should be understood that the following text is only used to describe one or several specific implementation manners of the present invention, and does not strictly limit the scope of protection of the specific claims of the present invention.

[0029] As Figure 1-8 shown, an algorithm for optimizing the relative position control of a robotic arm motor includes the following steps:

[0030] S1: Expand the state observer, and then perform disturbance estimation and acceleration estimation through the extended state observer, so as to serve as the preliminary work of the active disturbance rejection device and online inertia estimation. The specific operations are as follows:

[0031] S11: Record the controller torque current command Iq, perform data conversion and filtering processing with a low-pass filter with a bandwidth of λ to obtain iq0, and use iq0 for the disturbance estimation in S13 below. The processing flow is as shown in the appendix Figure 2 shown;

[0032] S12: Record the sensor feedback speed ω and perform acceleration estimation Acceleration estimation can be used for the estimation of the disturbing force in S13 below. The specific flow of this acceleration estimation is as shown in Figure 3 shown;

[0033] S13: Subtract iq0 in S11 from in S12 to obtain the disturbance force estimation value wherein, before subtracting iq0 and in S13 to obtain the disturbance force estimation value , it is necessary to multiply by and the obtained from the previous operation in S3 below. The specific flow is as shown in the appendix Figure 4 shown.

[0034] S2: Determine whether the online inertia estimation in S1 meets the condition of the minimum acceleration. The inertia estimation of the present invention needs to meet the condition of the minimum acceleration, and it is necessary to calculate and judge in real time whether the acceleration at each moment reaches the requirement. Therefore, two things need to be completed in this step. The steps are as follows:

[0035] S21: Calculate the minimum acceleration value a allowed for the inertia estimation min ;

[0036] Calculate the minimum acceleration a offline min When judging, the following inequality is used:

[0037]

[0038] where, ΔJ wmax is the maximum value of the difference between two inertia estimations, generally set between 0.3 and 0.6, which is a known quantity and is calculated according to 30% of the system. E is the minimum velocity resolution, which depends on the number of sensor lines N, and T s is the velocity loop control period, and the value is the same as T in the following text ASR The conversion formula is:

[0039]

[0040] where, T ASR is the velocity loop control period, and Π is the pi;

[0041] After sorting out and simplifying equations (1) and (2), we get:

[0042]

[0043] where, i is the number of times of judging the adjacent two inertia estimations, so the time interval is i*T ASR , k is the number of times of inertia estimation, here: the velocity measured at the kth time - the velocity measured at the k - i th time (with i control periods in between);

[0044] Through the above formula, it can be judged whether the current condition of the minimum acceleration of the inertia estimation is reached.

[0045] S22: The logic of judging in real time whether the acceleration meets the minimum acceleration condition.

[0046] Since the calculation is not performed in each cycle, therefore, first calculate the acceleration online Acceleration The calculation formula of is:

[0047]

[0048] where, i is the number of times of judgment.

[0049] Different from the acceleration in S1, the acceleration here is used for judgment rather than calculation.

[0050] S3: If the judgment in S2 meets the conditions, enter the online inertia estimation.

[0051] The formula for inertia estimation is:

[0052]

[0053] And:

[0054]

[0055] Where, is the inertia estimation value.

[0056] Converted to a flowchart as shown in Appendix Figure 5 shown. Among them, the structure within the dashed box is the inertia estimation process. To avoid too long integration time, the present invention uses the following state machine for operation to avoid data register overflow caused by too long integration time, as shown in Appendix Figure 6 shown;

[0057] A1: Set a rotational speed threshold Wmth, calculate the accumulated time of A1. If the rotational speed is greater than Wmth, move to A2. Wmth can ensure a wide enough speed change range for easy inertia identification. If the value in the time register is greater than the maximum time threshold Tmax, clear the time register and move to A4. The setting of Tmax can avoid the overflow of integration and the time register;

[0058] A2: Calculate the accumulated time of A2. If the value in the time register is greater than the time threshold Tth, transfer to state 3 and clear the time register. This can ensure a long enough calculation period for easy inertia identification;

[0059] A3: Calculate the accumulated time of A3. If zero speed or zero-crossing speed appears, update the observed inertia, clear the time register, and move back to state 1; if the value in the time register is greater than the maximum time threshold Tmax, clear the time register and move to state 4;

[0060] A4: Wait until the first zero speed or zero-crossing speed appears, then transfer to A1.

[0061] S4: According to the inertia estimated in S3, update the controller parameters in real time to improve the system's adaptive ability.

[0062] The specific formula is:

[0063]

[0064] where, ω sc is the speed loop bandwidth, with the unit of rad / s, K T is the torque constant, with the unit of N*m / A, u is the correction value of the speed bandwidth, and the actual bandwidth * u = the theoretical bandwidth;

[0065] Substitute Ki and Kp in Equation (7) into the PI controller in real time, so that the system has the ability to adapt to various inertias, and it is transformed into a control block diagram as shown in the appendix Figure 7 as follows.

[0066] S5: Use the disturbance estimation value for active disturbance rejection, and cooperate with S4 for disturbance compensation to improve the real-time response ability of the system. The specific flow chart is as shown in the appendix Figure 8 as follows.

[0067] Appendix Figure 8 In the figure, fdis is the disturbance quantity existing in the actual working condition, f is the disturbance estimation quantity, which can compensate the influence brought by fdis in real time, pref is the control target value of the active disturbance rejection device. Once the system gives a command p*, the active disturbance rejection device will regard it as a disturbance quantity of p - p* for compensation;

[0068] In the above figure, the part of the dotted box a is the key part of the active disturbance rejection device. Its function is to convert the command into a disturbance quantity and use the disturbance estimation ability to improve the system response ability. The dotted box b is the traditional passive disturbance rejection device. When a disturbance comes, this device can passively respond quickly to compensate the disturbance torque and improve the disturbance rejection ability of the system.

[0069] This application adopts an improved active disturbance rejection device, which can replace the feedforward control in some relative position control scenarios. Different from the feedforward control, this scheme does not depend on the known motion trajectory, and adopts an online inertia observation algorithm, which can update the controller parameters online in real time according to the inertia, greatly improving the adaptive ability of the whole system;

[0070] This application uses an extended state observer to observe the disturbance quantity, does not depend on the model, has strong robustness and response ability, and can greatly improve the system response ability without using feedforward control. This application regards the position reference as the disturbance result, makes full use of the observed disturbance quantity to optimize the response ability, and solves the problem that the feedforward optimization controller cannot be used in relative position control;

[0071] This application utilizes the active function of the disturbance observer, which can not only suppress external disturbances and compensate internal model errors, but also significantly improve the accuracy and response speed of position control. Compared with the traditional control algorithm, the present invention performs better in dealing with external disturbances, uncertainties and internal parameter changes, improving the overall robustness of the system;

[0072] Moreover, the present application simplifies the system design and implementation. Traditional control systems often require complex feedforward control designs to improve performance. However, the present invention simplifies the system design by actively utilizing a disturbance observer, and at the same time avoids problems such as noise introduced by the derivative process, reducing the complexity in the design and implementation process.

[0073] The above are only the preferred embodiments of the present invention. It should be noted that for those of ordinary skill in the art, without departing from the principle of the present invention, several improvements and refinements can be made, and these improvements and refinements should also be regarded as the protection scope of the present invention. The structures, devices, and operation methods not specifically described and explained in the present invention are implemented according to the conventional means in the art without special description and limitation.

Claims

1. An algorithm for optimizing the relative position control of a robot arm motor, characterized in that: The following steps are involved: S1: Expand the state observer, and then perform disturbance estimation and acceleration estimation through the expanded state observer, so as to serve as a preparatory work for active anti-disturbance device and inertia online estimation; The specific operation steps in S1 are: S11: Record the controller torque current command Iq, perform data conversion and filtering with a low-pass filter with a bandwidth of λ, and thus obtain iq0; S12: Record the sensor feedback speed ω and estimate the acceleration S13: Connect iq0 of S11 and iq1 of S12 Perform difference processing to obtain the estimated value of disturbance force Among them, iq0 and The disturbance force estimate is obtained by performing the difference processing. Previously, you needed to use Compared with the previous calculation in S3 below multiply; S2: Determine whether the online estimation of inertia in S1 satisfies the minimum acceleration condition; When determining in S2 whether the inertia online estimation in S1 satisfies the minimum acceleration condition, the steps are: S21: Calculate the minimum acceleration value a allowed for inertia estimation min ; The offline calculation of the minimum acceleration a in S21 min When judging, the following inequality is used: Among them, ΔJ wmax is the maximum value of the difference between the two inertia estimates, which is calculated based on the system selection of 30%, E is the minimum speed resolution, which depends on the number of sensor lines N, and T s is the speed loop control period, and the conversion formula is: Among them, T ASR is the speed loop control period, Π is the pi; Simplifying equation (1) and equation (2), we get: Among them, i is the number of consecutive inertia estimation judgments, so the time interval is i*T ASR , k is the number of inertia estimation; Through the above formula, it can be determined whether the condition of minimum acceleration of inertia estimation is currently met; S22: logic for determining in real time whether the acceleration meets the minimum acceleration condition; The logic of determining whether the acceleration satisfies the minimum acceleration condition in S22 is: First calculate the acceleration online Acceleration The calculation formula is: Among them, i is the number of judgments; S3: Estimate the inertia online; The formula for estimating inertia in S3 is: and: in, is the estimated value of inertia; S4: Update controller parameters in real time according to the inertia estimated in S3 to improve the system's adaptive capability; The specific formula in S4 is: Among them, ω sc is the speed loop bandwidth, K T is the torque constant, u is the correction value of the speed bandwidth; Bring Ki and Kp in equation (7) into the PI controller in real time, so that the system has the ability to adapt to various inertias; S5: Use the disturbance estimation value to perform active anti-disturbance and cooperate with S4 to perform disturbance compensation to improve the real-time response capability of the system.

Images