A two-degree-of-freedom planar robot arm control method and system based on nominal control items and uncertain items, and a storage medium

Abstract

A two-degree-of-freedom planar robot arm control method and system based on nominal control terms and uncertain terms, and a storage medium, which controls the robot arm by constructing a controller model, the controller model including nominal control terms and robust control terms, the nominal control terms being used to describe the determined part of the robot arm control, and the robust control terms being used to describe the uncertain part of the robot arm control. It has a simple structure, is easy to calculate the uncertain terms, and can quickly construct a two-degree-of-freedom planar robot arm controller model.

Description

TECHNICAL FIELD

[0001] The application relates to the technical field of mechanical arm control, in particular to a two-degree-of-freedom planar mechanical arm control method and system based on nominal control items and uncertain items and a storage medium. BACKGROUND

[0002] Mechanical arms are applied to various industrial production fields, and the control accuracy of the mechanical arms is the most important basic index in the technical field of mechanical arms. In actual control, the mechanical arms are disturbed by nonlinear uncertain items such as system parameter perturbation, external disturbance and time-varying friction, and it is very difficult to establish an accurate mechanical arm dynamics model. Robust control is widely used in actual engineering because it can compensate for structural and non-structural uncertainties in the system, and common robust control methods include H ∞ control, sliding mode control and adaptive robust control. However, H ∞ control can compensate for uncertainties, but it is not based on model design, which is not easy to achieve high speed and high accuracy in actual operation; similarly, sliding mode control will cause high-frequency jitter of the system due to its own characteristics; and adaptive robust control needs to design an adaptation law by adjusting the controller structure online, which is also relatively complex in practice.

[0003] A kind of mechanical arm trajectory tracking control method based on high-order sliding mode observer is disclosed in CN109927032A, which first establishes a mechanical arm dynamics model, takes damping friction as a lumped disturbance, then establishes a global integral fast terminal sliding mode surface according to the mechanical arm trajectory tracking error, determines the mechanical arm control torque according to the global integral fast terminal sliding mode surface, establishes a feedback high-order sliding mode observer according to the control torque, and estimates the lumped disturbance in an adaptive manner. The deficiencies are as follows: 1. Sliding mode control will cause high-frequency jitter of the system due to its own characteristics; 2. The lumped disturbance is estimated in an adaptive manner, which needs to design an adaptation law, and if the external environment changes, the adaptation law needs to be redesigned; 3. Damping friction is taken as a lumped disturbance item for estimation. SUMMARY

[0004] The first purpose of the application is to provide a two-degree-of-freedom planar mechanical arm control method based on nominal control items and uncertain items, which has a simple structure, is easy to calculate the uncertain items, and can quickly construct a two-degree-of-freedom planar mechanical arm controller model.

[0005] The first purpose of the application is achieved by the technical scheme, and the specific method is as follows:

[0006] 1) Obtain the model construction parameters of the two-degree-of-freedom mechanical arm to be controlled and the application environment parameters of the mechanical arm to be controlled, and give the ideal trajectory parameters of the mechanical arm;

[0007] 2) constructing a controller model of the control mechanical arm according to the two-degree-of-freedom mechanical arm model parameters, the application environment parameters and the ideal trajectory parameters;

[0008] The controller model comprises a nominal control item and a robust control item, the nominal control item is used for describing the determined part of the mechanical arm control, and the robust control item is used for describing the uncertain part of the mechanical arm control.

[0009] In the embodiment, the mechanical arm controller model is explicitly divided into the nominal control item and the robust control item, and is respectively used for describing the determined part and the uncertain part. For any determined two-degree-of-freedom mechanical arm and the application environment thereof, the nominal control item and the robust control item can be quickly determined, the structure is simple, easy to implement, and good control performance is exhibited in the simulation experiment.

[0010] As preferred, the nominal control item comprises a friction force control sub-item, which is used for quantifying the influence of the friction force on the controller model and reducing the uncertain part variable.

[0011] The friction force control sub-item is a segmented model, and whether the joint speed of the controlled mechanical arm is greater than the friction force model speed selection threshold is determined.

[0012] In the embodiment, the friction force is taken as an analysis object and is summarized into the nominal control item, so that the uncertain part can be reduced and the robust control item is facilitated to be constructed.

[0013] As preferred, the friction force control sub-item comprises an independent variable environment temperature T, which is used for quantifying the influence of the temperature on the friction force control sub-item, and corresponding friction force control sub-items are calculated in different environment temperature intervals.

[0014] In the embodiment, the variable caused by the change of the environment temperature is analyzed, the influence on the friction force is converted, the uncertain part variable caused by the temperature is further reduced, and the robust control item is facilitated to be constructed.

[0015] As preferred, the method further comprises step 3):

[0016] 3) collecting an actual running trajectory of the mechanical arm, when an error of the actual running trajectory occurs, firstly judging whether the size relationship between the joint limit speed of the mechanical arm and the friction force model speed selection threshold is incorrect, if incorrect, replacing the segment of the friction force control sub-item;

[0017] If incorrect, the independent variable environment temperature T is re-measured, and the friction force control sub-item is re-calculated according to the independent variable environment temperature.

[0018] In this embodiment, when the mechanical arm controller model has errors with the actual running trajectory, the errors caused by friction or temperature changes can be determined first, and if caused by friction or temperature changes, the errors can be quickly solved.

[0019] As preferred, the model construction parameters include:

[0020] The moment of inertia I1 of the connecting rod 1, the moment of inertia I2 of the connecting rod 2, the mass m1 of the connecting rod 1, the mass m2 of the connecting rod 2, the mass m3 of the joint 2 motor, the mass m4 of the end motor, the length l1 of the connecting rod 1, the length l2 of the connecting rod 2, the distance l c1 from the center of rotation of the mass center of the connecting rod 1 c2 , the distance l ci from the center of rotation of the mass center of the connecting rod 2 vi , the Coulomb friction coefficient f si ;

[0021] The ideal trajectory parameters include the two-degree-of-freedom planar mechanical arm trajectory signals θ d (t) and The current angle θ (t) of the first joint and the current angle of the second joint

[0022] The application environment parameters include the temperature T of the mechanical arm application environment.

[0023] As preferred, the control torque control model is:

[0024]

[0025] Wherein, is the control torque, is the nominal control term, is the robust control term, the independent variable t is time, θ is joint angle, is the joint angular velocity.

[0026] As preferred, the nominal control term is:

[0027]

[0028] In the formula, K P ∈R 2 is a proportional coefficient, which can be specifically expressed as K P =diag{[k pi ] 2×2}, k pi >0, i=1,2;

[0029] KD ∈ R 2 is the differential coefficient, which can be expressed as K D = diag{[k di ] 2×2}, k di > 0, i = 1, 2;

[0030] S = diag{[s i ] 2×2}, s i > 0, i = 1, 2, and s1= s2= 1, i.e., S = diag{1; 1};

[0031] θ d is the desired trajectory, is the desired joint angle acceleration, e is the joint angle error, is the joint angle velocity error, is the nominal inertia matrix, is the nominal Coriolis matrix, is the nominal friction matrix;

[0032] wherein,

[0033]

[0034] wherein,

[0035]

[0036] wherein,

[0037]

[0038] In the formula, f ci is the Coulomb friction coefficient, f vi (T) is the viscous friction coefficient, f si is the static friction coefficient, sign(·) is the sign function, ω s is the joint speed coefficient;

[0039]

[0040]

[0041] In the formula, k l , k p and k q are parameters that need to be determined according to the actual motion state, a i is the output acceleration of the corresponding motor;

[0042] The specific formula of the corresponding friction control subterm calculated from the independent variable environment temperature T is as follows:

[0043]

[0044] In the formula, T is the environment temperature, f vi , f mi and are the viscous friction coefficients in different temperature intervals, Q is the sensitivity coefficient of the viscous friction coefficient to temperature change, R is the influence coefficient of temperature on the size of the viscous friction coefficient, T1, T2 and T3 are temperature thresholds.

[0045] The nominal friction matrix and the formula of the corresponding friction of the independent variable environment temperature T are self-defined formulas conforming to the actual mechanical arm control model, and the friction and the temperature are summarized as nominal control terms, so that the nominal control terms of any two-degree-of-freedom mechanical arm in a determined temperature environment can be quickly constructed, and good control performance is exhibited in simulation experiments.

[0046] As a preferred, the robust control term is:

[0047]

[0048]

[0049] In the formula, γ and α are both positive design parameters determined according to engineering examples;

[0050] wherein is a speed error saturation function, and can be specifically expressed as

[0051]

[0052] In the formula, is the limit speed error of the joint, k m1 and k m2 are limit speed error threshold protection coefficients.

[0053] In this embodiment, after the friction and the temperature factors are excluded from the uncertain term, the uncertain term can be simply expressed, so that the robust control term of any two-degree-of-freedom mechanical arm in a determined temperature environment can be quickly constructed.

[0054] A second object of the present application is to provide a two-degree-of-freedom planar mechanical arm control system based on nominal control terms and uncertain terms, comprising:

[0055] A mechanical arm data initialization module is configured to obtain model construction parameters of a two-degree-of-freedom mechanical arm to be controlled and application environment parameters of the mechanical arm to be controlled, and give ideal trajectory parameters of the mechanical arm.

[0056] A mechanical arm controller model construction module is configured to establish a controller model for controlling the mechanical arm according to the two-degree-of-freedom mechanical arm model construction parameters, the application environment parameters and the ideal trajectory parameters.

[0057] A mechanical arm control module is configured to control the mechanical arm according to the controller model of the mechanical arm.

[0058] The mechanical arm control torque construction module comprises a nominal control term submodule and a robust control term submodule of the controller model; the nominal control term submodule is configured to generate a certain mathematical model for controlling the mechanical arm; and the robust control term submodule is configured to generate an uncertain mathematical model for controlling the mechanical arm.

[0059] A third object of the present application is to provide a storage medium storing a plurality of instructions suitable for being loaded by a processor to execute the above method.

[0060] Other advantages, objects, and features of the application will be better understood from the following specification taken in conjunction with the accompanying drawings. BRIEF DESCRIPTION OF DRAWINGS

[0061] The drawings of the present application are as follows.

[0062] Figure 1 The figure is a control flow diagram of the present application.

[0063] Figure 2 The figure is a simplified diagram of a two-degree-of-freedom planar mechanical arm.

[0064] Figure 3 The figure is a joint angle response curve of joint 1 under a step signal.

[0065] Figure 4 The figure is a joint angle response curve of joint 2 under a step signal.

[0066] Figure 5 The figure is a joint angle response curve of joint 1 under a sinusoidal signal.

[0067] Figure 6 The figure is a joint angle response curve of joint 2 under a sinusoidal signal.

[0068] Figure 7 The figure is a joint angle error response curve of joint 1 under a sinusoidal signal.

[0069] Figure 8 The figure is a joint angle error response curve of joint 2 under a sinusoidal signal. DETAILED DESCRIPTION

[0070] The application will be further described below in connection with the accompanying drawings and examples.

[0071] Given the ideal trajectory signal θ d (t) and The joint angle θ(t) and joint angle and the corresponding error e(t) and The general dynamic model of the two-degree-of-freedom planar manipulator system is established based on the Lagrange energy method, which is

[0072]

[0073] where the independent variable t is time, i represents the i-th joint variable, θ is the joint angle, is the joint velocity, τ(t) represents the control torque, is the Lagrange multiplier, which is calculated from the kinetic energy and potential energy, i.e.

[0074]

[0075] In the formula, is the system kinetic energy, and P(θ, t) is the system potential energy.

[0076] Since the two joints of the two-degree-of-freedom planar manipulator move in the horizontal plane, the potential energy is 0, i.e. P(θ, t) = 0.

[0077] And the system kinetic energy can be further expressed as

[0078]

[0079] where I1 and I2 are the moments of inertia of link 1 and link 2, m1 and m2 are the masses of the two links, m3 is the mass of the joint 2 motor, m4 is the mass of the end motor, l1 and l2 are the lengths of link 1 and link 2, l c1 and l c2 are the distances from the centers of mass of the two links to the rotation centers, respectively.

[0080] The Lagrange multiplier is brought into the Lagrange energy method to obtain the general dynamic model of the two-degree-of-freedom planar manipulator system, where the gravity matrix G(θ, t) = 0, and

[0081]

[0082] In the formula is the joint angular acceleration, J(θ, t) ∈ R 2×2is the inertia matrix, is the Coriolis and centrifugal force matrix, is the friction and additional disturbance:

[0083] Considering the uncertainty in the system, i.e. introducing an uncertainty parameter σ∈R n The dynamics model of the two-degree-of-freedom planar manipulator can be further written as

[0084]

[0085] The uncertainty in the formula is time-varying but bounded, and there exists a set such that is established.

[0086] The parameter matrix is divided into a nominal part and an uncertainty part, and according to the ideal joint angle θ d (t) and the joint angle error e(t), the parameter matrix is divided into a nominal part and an uncertainty part, i.e.

[0087]

[0088] Here, according to the dynamics parameters of the two-degree-of-freedom planar manipulator in step 3, we can get

[0089]

[0090] where

[0091]

[0092] In addition,

[0093]

[0094] where,

[0095]

[0096] A two-degree-of-freedom planar manipulator, the values of each parameter are as shown in the following table:

[0097]

[0098] Finally,

[0099]

[0100] In the formula, f ci is the Coulomb friction coefficient, f vi (T) is the viscous friction coefficient, f si is the static friction coefficient, sign(·) is the sign function, ω s is the joint speed coefficient;

[0101]

[0102]

[0103] where k l , k p and k q are parameters to be determined according to the actual motion state, a i is the output acceleration of the corresponding motor.

[0104] The specific formula of the corresponding friction force control subterm calculated from the independent variable ambient temperature T is:

[0105]

[0106] where T is the ambient temperature, f vi , f mi and are the viscous friction coefficients in different temperature intervals, Q is the sensitivity coefficient of the viscous friction coefficient to temperature change, R is the influence coefficient of temperature on the size of the viscous friction coefficient, and T1, T2 and T3 are temperature thresholds.

[0107] The values of the friction coefficients are shown in the following table:

[0108]

[0109] Further, since the uncertainty is bounded, ρ is determined as the upper bound of the comprehensive influence of all uncertainties, i.e.

[0110]

[0111] where ||·|| is the Euclidean norm, which includes all uncertainties, and can be expressed as

[0112]

[0113] where S = diag{[s i ] 2×2}, s i > 0, i = 1, 2

[0114] Specifically, s1 = s2 = 1, i.e. S = diag{1; 1}.

[0115] Constructing a nominal control term based on the model and error and a robust control term i.e. the control torque can be expressed as

[0116]

[0117] Further, the nominal control term is

[0118]

[0119] where K P ∈ R 2 is a proportional coefficient, which can be specifically expressed as

[0120] K P = diag{[k pi ] 2×2}, k pi > 0, i = 1, 2

[0121] Specifically, take k p1 = 950 and k p2 = 550, i.e., K P = diag{950; 550}.

[0122] K D ∈ R 2 is a differential coefficient, which can be specifically expressed as

[0123] K D = diag{[k di ] 2×2}, k di > 0, i = 1, 2

[0124] Specifically, take k d1 = 120 and k p2 = 35, i.e., K P = diag{120; 35}.

[0125] Further, the robust control term is

[0126]

[0127] where

[0128] where take a = 0.1 and g = 0.01.

[0129] The simulation analysis is performed on a two-degree-of-freedom planar manipulator system, in which ideal step signals and ideal sinusoidal signals are respectively taken as reference signals to verify the transient performance and steady-state performance of the control algorithm. In order to better show the excellent performance of the proposed control method, the model-based nominal control term is compared, and the results are analyzed. The ideal step signal is

[0130]

[0131] The ideal sinusoidal signal is

[0132]

[0133] In addition, the key dynamic parameters of the two-degree-of-freedom planar manipulator are affected by a small periodicity, which can be expressed as

[0134] Δm1= Δm2= 0.03sin(0.1t)

[0135] Δm3= Δm4= 0.05sin(0.1t)

[0136] ΔI1= ΔI2= 0.01sin(0.1t)

[0137] ΔF1= 0.035sin(t)

[0138] ΔF2= 0.015sin(t)

[0139] In addition, in order to distinguish the simulation results of the sinusoidal signal, let the initial error be

[0140]

[0141] The step simulation results are shown in FIG. 4, and the sinusoidal tracking simulation results are shown in FIG. 5. Figures 3-4 Figures 5-8

[0142] Figure 3 and Figure 4 are the joint angle response curves of the two joints under the step signal. For transient performance, the corresponding evaluation index data is shown in the following table, in which T r and T s are the rise time and steady time, respectively, and IM(·) is the improvement amplitude.

[0143]

[0144] Figure 7 and Figure 8 are the joint angle response curves of the two joints under the sinusoidal signal. Figure 5 and Figure 6 are the joint angle error response curves of the two joints under the sinusoidal signal. The maximum value MAXE and the root mean square RMSE of the error are used as the evaluation criteria, which can be expressed as

[0145] MAXE = max(|e i |), i = 1, 1…, 20000

[0146]

[0147] ​​The specific data is shown in the following table

[0148]

[0149] From the simulation results of the step signal and the sine signal, it can be seen that the proposed robust control algorithm can ensure its transient performance while having excellent steady-state performance, and has strong robustness under the influence of simulation uncertainty, and is capable of resisting internal uncertainty or external interference of the system.

[0150] Those skilled in the art will understand that embodiments of the present application can be provided as methods, systems, or computer program products. Therefore, the present application can take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application can take the form of a computer program product implemented on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROMs, optical storage, etc.) containing computer usable program code.

[0151] The present application is described with reference to flowcharts and / or block diagrams according to the methods, devices (systems), and computer program products of embodiments of the present application. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, as well as combinations of flows and / or blocks in the flowcharts and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing apparatus to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing apparatus produce an apparatus that implements the functions specified in the flowcharts and / or block diagrams. Figure 1 one or more flows and / or blocks Figure 1 an apparatus with the functions specified in one or more flows and / or blocks.

[0152] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to work in a specific manner, so that the instructions stored in the computer-readable memory produce a manufactured product including instruction apparatus, which implements the functions specified in the flowcharts and / or block diagrams. Figure 1 one or more flows and / or blocks Figure 1 an apparatus with the functions specified in one or more flows and / or blocks.

[0153] These computer program instructions can also be loaded onto a computer or other programmable data processing apparatus, so that a series of operation steps are performed on the computer or other programmable data processing apparatus to produce a computer-implemented process, so that the instructions executed on the computer or other programmable data processing apparatus provide a process for implementing the functions specified in the flowcharts and / or block diagrams. Figure 1 one or more flows and / or blocks Figure 1 an apparatus with the functions specified in one or more flows and / or blocks.

[0154] It should be pointed out finally that the above embodiments are only used to illustrate the technical solutions of the present application but not to limit it. Although the present application has been described in detail with reference to the above embodiments, it should be understood by those skilled in the art that the specific embodiments of the present application can be modified or equivalently replaced without departing from the spirit and scope of the present application, and any modification or equivalent replacement should be covered in the protection scope of the claims of the present application.

Claims

1. A two-degree-of-freedom planar manipulator control method based on a nominal control term and an uncertainty term, characterized by, The specific method is as follows: 1) Obtain the model construction parameters of the two-degree-of-freedom planar manipulator to be controlled and the application environment parameters of the manipulator to be controlled, and give the ideal trajectory parameters of the manipulator; 2) According to the two-degree-of-freedom planar manipulator model construction parameters, the application environment parameters and the ideal trajectory parameters, a controller model of the manipulator is established; Wherein, the controller model includes a nominal control term and a robust control term, the nominal control term is used to describe the deterministic part of the manipulator control, and the robust control term is used to describe the uncertain part of the manipulator control; The nominal control term includes a friction force control subterm, which is used to quantify the influence of friction force on the controller model and reduce the uncertain part variables; The friction force control subterm is a segmented model, which is determined by whether the joint speed of the manipulator to be controlled is greater than the friction force model speed selection threshold value; The friction force control subterm includes an independent variable environmental temperature T, which is used to quantify the influence of temperature on the friction force control subterm, and the corresponding friction force control subterm is calculated in different environmental temperature intervals.

2. The two-degree-of-freedom planar manipulator control method based on a nominal control term and an uncertainty term according to claim 1, wherein The method further includes step 3): 3) Collect the actual running trajectory of the manipulator, when the actual running trajectory has an error, first judge whether the size relationship between the joint limit speed of the manipulator and the friction force model speed selection threshold value is wrong, if it is wrong, replace the segmentation of the friction force control subterm; If it is not wrong, the independent variable environmental temperature T is measured again, and the friction force control subterm is recalculated according to the independent variable environmental temperature.

3. The two-degree-of-freedom planar manipulator control method based on a nominal control term and an uncertainty term according to claim 1, wherein The model construction parameters include: I1: moment of inertia of link 1, I2: moment of inertia of link 2, m1: mass of link 1, m2: mass of link 2, m3: mass of joint 2 motor, m4: mass of end motor, l1: length of link 1, l2: length of link 2, l: distance from center of mass of link 1 to center of rotation c1 , l: distance from center of mass of link 2 to center of rotation c2 ; f: Coulomb friction coefficient ci , f: viscous friction coefficient vi , f: static friction coefficient si ; The ideal trajectory parameters include a two-degree-of-freedom planar robot arm desired trajectory signal, a current angle θ(t) and a current angular velocity The corresponding error e(t) is obtained and The application environment parameters include the independent variable environmental temperature T of the manipulator application environment.

4. The two-degree-of-freedom planar manipulator control method based on a nominal control term and an uncertainty term according to claim 3, wherein The control torque control model is: wherein, is a control torque, is a nominal control term, is a robust control term, the argument t is time, and θ is the joint angle, is the joint angular velocity.

5. The two-degree-of-freedom planar manipulator control method based on a nominal control term and an uncertainty term according to claim 4, wherein The nominal control term Is: where K P ∈ R 2 is a proportionality coefficient, which can be expressed as K P = diag{[k pi ] 2×2}, k pi > 0, i = 1, 2. K D ∈R 2 is a differential coefficient, which can be expressed as K D = diag{[k di ] 2×2},k di > 0, i = 1, 2; S = diag{ [s i ] 2×2},s i > 0, i = 1,2, take si = s2 = 1, i.e. S = diag{1;1}; θ d is the desired trajectory, is the desired joint angular acceleration, e is the joint angular error, is the joint angular velocity error, is the nominal inertia matrix, is the nominal Coriolis force matrix, is the nominal friction matrix; wherein wherein Wherein, where sign(·) is the sign function, ω s is the joint velocity coefficient; where k l , k p , and k q are parameters that need to be determined according to the actual motion state, and a i is the output acceleration of the motor corresponding to the parameter. The specific formula for calculating the corresponding friction force control subterm of the independent variable environmental temperature T is: In the formula, f vi , f mi and are the viscous friction coefficients in different temperature intervals, Q is a sensitivity coefficient of the viscous friction coefficient to temperature change, R is an influence coefficient of temperature on the size of the viscous friction coefficient, T1, T2 and T3 are temperature thresholds.

6. The two-degree-of-freedom planar manipulator control method based on a nominal control term and an uncertainty term according to claim 4, wherein The robust control item Is: In the formula, γ and α are both positive design parameters determined according to engineering examples; wherein is a velocity error saturation function, which can be expressed in particular as wherein is the joint's limit velocity error, k m1 and k m2 is the limit velocity error threshold protection coefficient.

7. A storage medium, characterized by The storage medium stores a plurality of instructions, and the instructions are suitable for being loaded by the processor to execute the method of any one of claims 1 to 6.

Images