A joint space trajectory tracking control method for a serial manipulator

Abstract

The present invention provides a joint space trajectory tracking control method for a serial manipulator. The method comprises the following steps: using the Lagrange equations to establish the dynamic equations of an n-degree-of-freedom serial rigid manipulator system; using an inner-loop inverse dynamics controller to simplify the original nonlinear multiple-input multiple-output (MIMO) tracking error system into a set of decoupled linearized single-input single-output (SISO) tracking error systems; designing a robust model predictive tracking control (D-RMPTC) scheme based on a disturbance observer; verifying the main theoretical results with reference to the proposed controller design; proving the recursive feasibility and closed-loop stability, and then applying the proposed control scheme to a simulation study of a PUMA 560 manipulator. By comparing it with the traditional RMPTC scheme, the time evolution of the angular position and velocity of the six joints under the two control schemes is obtained. The present invention can achieve high-precision tracking control with good disturbance attenuation and constraint satisfaction within the disturbance tolerance range.

Description

Technical Field

[0001] The present invention relates to the technical field of industrial robot control, and in particular to a joint space trajectory tracking control method of a serial robot arm. Background Art

[0002] The development of industrial robots has been driven by the latest industrial revolution, Industry 4.0. The inventory of industrial robots in factories around the world is on the rise. The widespread use of robotic arms in industry not only reduces production costs and improves efficiency, but also offers unmatched precision. However, the accuracy of robotic arm trajectory tracking significantly impacts product quality and production profitability, making research on robotic arm trajectory tracking of significant economic significance.

[0003] Trajectory tracking control of robotic arms in Cartesian / joint space is a widely studied topic in the industry and has attracted widespread attention. Serial robotic arms are among the most popular industrial robots, widely used to manipulate biohazardous or radioactive objects and perform repetitive tasks with efficiency and accuracy far exceeding human performance.

[0004] Currently, the control method based on disturbance observer in the prior art does not directly consider all joint states and control constraints of the robot arm, and therefore cannot guarantee the satisfaction of the constraints. Summary of the Invention

[0005] An embodiment of the present invention provides a joint space trajectory tracking control method for a serial robot arm, so as to achieve trajectory tracking control of the serial robot arm with good controller performance.

[0006] In order to achieve the above-mentioned purpose, the present invention adopts the following technical solutions.

[0007] A joint space trajectory tracking control method for a serial robot arm, comprising:

[0008] The original nonlinear multi-input multi-output (MIMO) tracking error system is simplified into a set of decoupled linearized single-input single-output (SISO) tracking error systems using the inner loop inverse dynamics controller, and the set of decoupled linearized SISO tracking error systems is used as the inverse dynamics control input.

[0009] A predictive tracking control (D-RMPTC) scheme for a robust model of a serial manipulator based on a disturbance observer is designed. The D-RMPTC scheme is used to solve a constrained optimization problem and obtain the additive sum of the disturbance compensation input and the prediction input.

[0010] The inverse dynamics control input, interference compensation input and prediction input are additively formed into the joint torque of the n-degree-of-freedom serial rigid manipulator system. The joint torque of the n-degree-of-freedom serial rigid manipulator system is used to establish the dynamic equation of the n-degree-of-freedom serial rigid manipulator system using the Lagrange equation. By solving the dynamic equation of the n-degree-of-freedom serial rigid manipulator system, the real-time angle information of the manipulator joint is obtained.

[0011] Preferably, the inner-loop inverse dynamics controller simplifies the original nonlinear multi-input multi-output (MIMO) tracking error system into a set of decoupled linearized single-input single-output (SISO) tracking error systems, and uses the set of decoupled linearized SISO tracking error systems as inverse dynamics control input, including:

[0012] The unperturbed reference trajectory is generated by the second-order integrator dynamics:

[0013]

[0014] in, is the reference state, is τ ref ∈T reference control input;

[0015] The state space form of the dynamic equation of the above series rigid manipulator system is:

[0016]

[0017] in It indicates status. Defined as the equivalent lumped perturbation including uncertainty.

[0018] By subtracting (1) from (2), we get the tracking error dynamics in the state space:

[0019]

[0020] in is the tracking error state. That is: x i =x i,ref +x i,e ,i∈Ι 1:2 .

[0021] Using inverse dynamics control, the MIMO system is reduced to n SISO decoupled linearized systems, one for each joint of the rigid manipulator system in series, and the control input τ is chosen as:

[0022] τ=M m (x 1,e +x 1,ref )(u e +u ref)+N m (x 1,e +x 1,ref ,x 2,e +x 2,ref ) (4)

[0023] in As the auxiliary control input, substitute (4) into (3) to obtain:

[0024]

[0025] The tracking error system of the j-th joint is obtained as:

[0026]

[0027] The tracking error system is written in a compact form as a matrix:

[0028]

[0029] in:

[0030]

[0031] The control input is Given, where u j,edob and u j,empc The paths are generated by the disturbance observer and RMPTC controller respectively.

[0032] Preferably, the design is based on a predictive tracking control (D-RMPTC) scheme for a robust model of a serial manipulator with a disturbance observer, and the D-RMPTC scheme is used to solve a constrained optimization problem to obtain the additive sum of the disturbance compensation input and the prediction input, including:

[0033] u j,edob Design:

[0034] Design disturbance observer equivalent lumped disturbance ω j :

[0035]

[0036] Among them L j is the observation gain selected by the designer; j is an auxiliary variable; ω j is the equivalent lumped disturbance estimate, and its initial value is set to ω j (t0) = 0;

[0037] The equivalent lumped disturbance estimation error is defined as Its dynamics can be obtained:

[0038]

[0039] For the matching disturbance in the system, the disturbance compensation is designed as follows:

[0040]

[0041] The closed-loop system is obtained as:

[0042]

[0043] u j,empc Design:

[0044] Using RMPTC to suppress residual disturbance ω j,e To achieve controller performance, we apply standard MPTC to a nominal system with tight state and control constraints, starting from the nominal system with the perturbation system as:

[0045]

[0046] in and are the nominal state and control input, respectively;

[0047] The event sampling interval is δ (δ = t k+1 -t k ,t k >t0) indicates the sampling event (x j,e (t k ),t k ) at the nominal finite-horizon optimization problem:

[0048]

[0049] in It's about Minimize the quadratic cost function, is the stage cost function, is the terminal cost function, tightening the state constraint set Control constraint set and the terminal state constraint set They are defined as:

[0050]

[0051] The optimal control input trajectory and its corresponding optimal predicted state trajectory are obtained by solving the optimization problem.

[0052] Preferably, the method further comprises:

[0053] The constraints in the constrained optimization problem include:

[0054] For a nominal system, there exists Q j ,R j ,P j And make A+Bk f Hurwitz stable feedback gain k f Make For control input is control-invariant, the inequality Applicable to any in

[0055] According to the disturbance observer dynamics and the disturbance estimation error dynamics, we can obtain:

[0056]

[0057] If the system controlled by the control input and the system controlled by the control input The nominal system of control evolves from the same initial state values, i.e. In the time interval [t k ,t k +T], the deviation between the actual state and the nominal best predicted state trajectory satisfies:

[0058]

[0059] in

[0060] Preferably, the inverse dynamics control input, the disturbance compensation input, and the prediction input are additively formed into the joint torque of the n-degree-of-freedom serial rigid manipulator system, and the dynamic equation of the n-degree-of-freedom serial rigid manipulator system is established using the Lagrange equation using the joint torque of the n-degree-of-freedom serial rigid manipulator system, including:

[0061] The inverse dynamics control input, the disturbance compensation input, and the prediction input are additively formed into the joint torque of the n-DOF serial rigid manipulator system. The dynamic equation of the n-DOF serial rigid manipulator system is written in the configuration space using the Lagrange equation using the joint torque of the n-DOF serial rigid manipulator system:

[0062]

[0063] in, and are the angular position, velocity and acceleration of the joint, is the joint space inertia matrix, represents the centripetal force matrix and Coriolis moment, represents the gravitational torque, is Coulomb friction and viscous damping, is the joint control torque, is the possible external disturbance torque acting on the joint;

[0064] The dynamic equations of the serial rigid manipulator system are expressed as follows:

[0065]

[0066] Among them, M m (θ) and It is calculated based on the actual measured mechanical dynamic parameters, which are M(θ) and The estimated values ​​of , ΔM(θ) and is the corresponding additional uncertainty;

[0067] The dynamic equations of the serial rigid manipulator system are rewritten into the following state space form:

[0068]

[0069] in It indicates the status. Defined as an equivalent lumped perturbation containing uncertainty, the constraints given by the system are:

[0070]

[0071] Where n is the number of joints of the serial manipulator, χ, T and W are convex and compact sets. 1j ,η 2j >0 is a constant estimated from experimental trials. are the joint limits for a particular manipulator.

[0072] Preferably, obtaining the real-time angle information of the manipulator joints by solving the dynamic equations of the n-degree-of-freedom serial rigid manipulator system includes:

[0073] Based on the Euler forward numerical integration method, the joint angle information of the robotic arm joint at the next moment is calculated by solving the dynamic equation of the n-degree-of-freedom series rigid robotic arm system according to the input joint torque.

[0074] It can be seen from the technical solutions provided by the above-mentioned embodiments of the present invention that the present invention proposes a robust hierarchical multi-loop composite control strategy for the spatial trajectory tracking of the joints of a general serial manipulator, which achieves trajectory tracking control with good controller performance while suppressing residual disturbances / uncertainties.

[0075] Additional aspects and advantages of the present invention will be set forth in part in the following description, will be obvious from the following description, or may be learned by practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0076] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0077] Figure 1 A flowchart of a joint space trajectory tracking control method of a serial manipulator based on inverse dynamics and disturbance observer provided by an embodiment of the present invention;

[0078] Figure 2 A flow chart of a simulation model of PUMA560 in MATLAB provided by an embodiment of the present invention;

[0079] Figure 3 A flow chart of geometric and inertial parameters of PUMA560 provided in an embodiment of the present invention;

[0080] Figure 4 A flowchart of the maximum absolute value of the PUMA560 state trajectory and control trajectory provided by an embodiment of the present invention;

[0081] Figure 5 A PUMA560 joint limit flow chart provided in an embodiment of the present invention;

[0082] Figure 6 An embodiment of the present invention provides an n ij ,Q j Flowchart of estimated values;

[0083] Figure 7 A flowchart of the time evolution of six joint angle positions in a robust model predictive tracking control method for a series manipulator based on inverse dynamics and disturbance observer provided by an embodiment of the present invention;

[0084] Figure 8 A flowchart of the time evolution of the angular velocities of six joints in a robust model predictive tracking control method for a series manipulator based on inverse dynamics and a disturbance observer provided by an embodiment of the present invention;

[0085] Figure 9 A flowchart of the time evolution of disturbance estimation error in a robust model predictive tracking control method for a series manipulator based on inverse dynamics and a disturbance observer provided by an embodiment of the present invention;

[0086] Figure 10 A schematic diagram comparing the quantitative performance indicators of a D-RMPTC solution and an RMPTC solution provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0087] The embodiments of the present invention are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and are not to be construed as limiting the present invention.

[0088] It will be understood by those skilled in the art that, unless expressly stated otherwise, the singular forms "a", "an", "said" and "the" used herein may also include the plural forms. It should be further understood that the term "comprising" used in the description of the present invention refers to the presence of the features, integers, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or groups thereof. It should be understood that when we refer to an element as being "connected" or "coupled" to another element, it may be directly connected or coupled to the other element, or there may be intermediate elements. In addition, "connected" or "coupled" as used herein may include wireless connections or couplings. The term "and / or" used herein includes any unit and all combinations of one or more associated listed items.

[0089] It will be understood by those skilled in the art that, unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by those skilled in the art in the art to which the present invention pertains. It should also be understood that terms such as those defined in common dictionaries should be understood to have meanings consistent with their meanings in the context of the prior art and, unless defined as such herein, will not be interpreted in an idealized or overly formal sense.

[0090] To facilitate understanding of the embodiments of the present invention, several specific embodiments will be further explained below with reference to the accompanying drawings. However, each embodiment does not constitute a limitation on the embodiments of the present invention.

[0091] In order to solve the joint space trajectory tracking control problem of a general serial robot, the present embodiment provides a predictive tracking control method for a serial manipulator robust model based on inverse dynamics and disturbance observer. The processing flow is as follows: Figure 1 As shown, the following steps are included:

[0092] Step S1: Use the inner loop inverse dynamics controller to simplify the original MIMO (Multiple-Input Multiple-Output) tracking error system into a set of decoupled linearized SISO (Simple Input Simple Output) tracking error systems, and use the above set of decoupled linearized SISO tracking error systems as the inverse dynamics control input.

[0093] Step S2: Design a predictive tracking control (D-RMPTC) scheme for a robust model of a serial manipulator based on a disturbance observer, and use the D-RMPTC scheme to solve the constrained optimization problem to obtain the additive sum of the disturbance compensation input and the prediction input.

[0094] With reference to the proposed controller design, the tightened state and control input in the optimization problem are derived to obtain the constraints in the above constrained optimization problem, which include the tightened state and control input constraints.

[0095] Step S3, using the Lagrange equation to establish the dynamic equation of the n-degree-of-freedom series rigid manipulator system;

[0096] The above-mentioned inverse dynamics control input, interference compensation input and prediction input are additively formed into the joint torque of the n-degree-of-freedom serial rigid manipulator system, and the Lagrange equation is used to establish the dynamic equation of the n-degree-of-freedom serial rigid manipulator system using the above-mentioned joint torque of the n-degree-of-freedom serial rigid manipulator system.

[0097] Step S4: obtaining the real-time angle information of the robotic arm joints by solving the dynamic equations of the above n-DOF series rigid robotic arm system.

[0098] Step S5: Prove the recursive feasibility and closed-loop stability of the dynamic equations of the above n-DOF series rigid manipulator system.

[0099] The control scheme proposed by the method of the present invention is applied to the simulation study of the PUMA 560 robotic arm. By comparing it with the traditional RMPTC scheme, the time evolution of the angular position and velocity of the six joints under the two control schemes is obtained, thereby providing theoretical analysis and feasibility assurance for the control scheme proposed by the method of the present invention.

[0100] Step S1 further includes the following sub-steps:

[0101] S1.1. The undisturbed reference trajectory is generated by the second-order integrator dynamics:

[0102]

[0103] in For reference status. is τ ref ∈T is the reference control input.

[0104] The state space form of the dynamic equation of the above series rigid manipulator system is:

[0105]

[0106] in It indicates status. Defined as the equivalent lumped perturbation including uncertainty.

[0107] By subtracting (1) from (2), we get the tracking error dynamics in the state space:

[0108]

[0109] in is the tracking error state. That is: x i =x i,ref +x i,e ,i∈Ι 1:2 .

[0110] S1.2. Using inverse dynamics control, the MIMO system is reduced to n SISO decoupled linearized systems, one for each joint. The control input τ is chosen as:

[0111] τ=M m (x 1,e +x 1,ref )(u e +u ref )+N m (x 1,e +x 1,ref ,x 2,e +x 2,ref ) (4)

[0112] in is the auxiliary control input. Substituting (4) into (3) yields:

[0113]

[0114] The tracking error system of the j-th joint is obtained as:

[0115]

[0116] It can be written in a compact form as a matrix:

[0117]

[0118] in:

[0119]

[0120] The control input is Given, where u j,edob and u j,empc The paths are generated by the disturbance observer and RMPTC controller respectively.

[0121] Step S2 further includes the following sub-steps:

[0122] S2.1、u j,edob Design:

[0123] Design disturbance observer equivalent lumped disturbance ω j :

[0124]

[0125] Among them L j is the observation gain selected by the designer; j is an auxiliary variable; ω j is the equivalent lumped disturbance estimate, and its initial value is set to ω j (t0)=0.

[0126] The equivalent lumped disturbance estimation error is defined as Then its dynamics can be obtained:

[0127]

[0128] For the matching disturbance in the system, the disturbance compensation is designed as follows:

[0129]

[0130] The closed-loop system is obtained as:

[0131]

[0132] S2.2、u j,empc Design:

[0133] Using RMPTC to suppress residual disturbance ω j,e , achieving the desired controller performance. The basic idea of ​​RMPTC is to apply standard MPTC to a nominal system with tight state and control constraints. Starting from the nominal system (no disturbance) of the perturbed system, it is:

[0134]

[0135] in and are the nominal state and the control input respectively.

[0136] S2.3. Design of optimization problem:

[0137] The event sampling interval is δ (δ = t k+1 -t k ,t k >t0) indicates the sampling event (x j,e (t k ),t k ) at the nominal finite-horizon optimization problem:

[0138]

[0139] in It's about Minimize the quadratic cost function. is the stage cost function, is the terminal cost function. Tighten the state constraint set Control constraint set and the terminal state constraint set They are defined as:

[0140]

[0141] By solving the optimization problem, the optimal control input trajectory and its corresponding optimal predicted state trajectory can be obtained.

[0142] The constraints in the above constrained optimization problem in step S2 include the following sub-steps:

[0143] S2.4. For a nominal system, there exists Q j ,R j ,P j And make A+Bk f Hurwitz stable feedback gain k f Make For control input is control-invariant. Applicable to any in

[0144] S2.5. According to the dynamics of the disturbance observer and the dynamics of the disturbance estimation error, we can obtain:

[0145]

[0146] S2.6. If the system controlled by the control input and the system controlled by the control input The nominal system of control evolves from the same initial state value (i.e. ), in the time interval [t k ,t k +T], the deviation between the actual state and the nominal best predicted state trajectory satisfies:

[0147]

[0148] in

[0149] Step S3 further includes the following sub-steps:

[0150] The dynamic equations of the n-degree-of-freedom serial rigid manipulator system are written in the configuration space using the Lagrange equations:

[0151]

[0152] in, and are the angular position, velocity and acceleration of the joint respectively. is the joint space inertia matrix. represents the centripetal force matrix and the Coriolis moment. represents the gravitational torque. is the Coulomb friction and viscous damping. is the joint control torque. is the possible external disturbance torque acting on the joint.

[0153] The dynamic equations of the serial rigid manipulator system can be expressed as uncertain:

[0154]

[0155] Among them, M m (θ) and It is calculated based on the actual measured mechanical dynamic parameters, which are M(θ) and Estimated value of ΔM(θ) and is the corresponding additional uncertainty.

[0156] The dynamic equations of the above series rigid manipulator system can be rewritten in state space form:

[0157]

[0158] in It indicates status. is defined as an equivalent lumped perturbation containing uncertainty. The constraints given by the system are:

[0159]

[0160] Where n is the number of joints of the serial manipulator, χ, T and W are convex and compact sets. 1j ,η 2j >0 is a constant estimated from experimental trials. are the joint limits for a particular manipulator.

[0161] Step S4 further includes the following sub-steps:

[0162] S4.1. For a perturbed system, if the optimization problem is at the initial instant t o The proposed DRMPTC scheme is recursively feasible if there is a feasible solution that satisfies the following conditions:

[0163]

[0164] In t k+1 Instantaneous candidate control input The structure is:

[0165]

[0166] in is a candidate control input The generated predicted state trajectory,

[0167]

[0168] The analysis results are:

[0169]

[0170] This means that when the control input is at t k+1 +T, Enter the collection Then we analyze and sort out:

[0171] In summary, the candidate control input is the optimization problem sampling event x j,e (t k+1 ,t k+1 ) is a feasible solution. As can be seen from S4.1 and S5.2, by properly designing the parameter Q j ,R j ,δ and T, can guarantee the recursive feasibility of the proposed D-RMPTC scheme.

[0172] S5.2. Under the proposed D-RMPTC scheme, if the conditions in S5.1 are met, the closed-loop system is a regional ISpS and also a tracking error system:

[0173] for Choose a candidate Lyapunov function as:

[0174]

[0175] in is the actual state at instant t, yes The control input, is The corresponding predicted state trajectory is given.

[0176] Analysis and collation Therefore, the closed-loop system is stable. If the system is in Ω with respect to ω j If there exists an ISpS-Lyapunov function, then the system is ISpS in Ω, and ||x j,e (∞,x j,e (t0),w j )||Ξ=0.

[0177] Next, in order to verify the effectiveness of the robust model predictive tracking control method for a series manipulator based on inverse dynamics and disturbance observer provided in this embodiment, a simulation experiment is conducted using MATLAB and a detailed description is given.

[0178] The simulation study of the control scheme proposed in the embodiment applied to the PUMA 560 manipulator was performed using the Robot Toolbox in MATLAB on a laptop computer with a processor running at 2.21 GHz and 8 GB of RAM. The sampling time of the simulation study was set to 0.05 s. The gravity torque, friction torque, centripetal torque, and Coriolis torque can be calculated by importing the real PUMA 560 parameters ( Figure 3 ) and the built-in functions in the Robot Toolbox. The simulation model of PUMA 560 is shown in Figure 2 As shown, it is based on the measured geometric and inertial parameters ( Figure 3 ) is established, and the simulation environment is close to the actual situation. The maximum absolute values ​​of the state trajectory and control torque trajectory generated by the D-RMPTC scheme are as follows: Figure 4 shown.

[0179] In this simulation experiment, the control goal is to follow the cubic polynomial reference trajectory x in the joint space. 1j,ref =a 5j t 5 +a 4j t 4 +a 3j t 3 +a 2j t 2 +a 1j t+a 0j ,j∈Ι 1:6 , satisfying that the initial angular velocity and acceleration are both zero, duration t pWhen the time is set to 10s, the joint is controlled from the initial angular position (0,π / 6,-π / 6,0,π / 6,π / 4) to the target angular position (π / 3,0,0,π / 3,0,π / 6). The disturbance observer gain is set to L j =150η 2j , achieving satisfactory estimation performance. The main parameters of the RMPTC scheme are determined as follows:

[0180] δ=0.05s,T=50δ,Q j =diag{10,10},R j =0.4,k f =[5.0005.9161] T

[0181]

[0182] ε1=4.6233, ε2=4.5115, ε3=4.2769, ε4=5.6491, ε5=5.2650, ε6=4.1992

[0183] And introduce the quantitative performance indicators mentioned in (i.e., the root mean square tracking error (RMSTE) and the root mean square control input (RMSC)), where N is the number of sampling steps of the simulation.

[0184]

[0185] The superiority of the D-RMPTC scheme is demonstrated by comparing it with the traditional RMPTC scheme. Both are implemented on the inverse dynamic linearization tracking error system. The second one is performed as shown in Figure 2. Figure 5 and Figure 6 The time evolution of the angular position and velocity of the six joints under the two control schemes are shown. For all joints, the trajectory generated by the D-RMPTC scheme is significantly closer to the reference trajectory than the RMPTC scheme. This means that the D-RMPTC scheme can guarantee higher tracking accuracy because the designed disturbance observer effectively estimates and compensates for the disturbance ( Figure 7 ).from Figure 8 It can be seen that for each joint, the RMSC values ​​under the two control schemes are almost close, while the RMSTE value under the D-RMPTC scheme is significantly smaller than that under the RMPTC scheme, which further proves the superiority of the D-RMPTC scheme.

[0186] Figure 9 A flowchart of the time evolution of disturbance estimation error in a robust model predictive tracking control method for a series manipulator based on inverse dynamics and a disturbance observer provided by an embodiment of the present invention; Figure 10A schematic diagram comparing the quantitative performance indicators of a D-RMPTC solution and an RMPTC solution provided by an embodiment of the present invention. Figure 9 and Figure 10 Additional explanation

[0187] The above analysis proves the effectiveness of the robust model predictive tracking control method for the series manipulator based on inverse dynamics and disturbance observer provided in this embodiment.

[0188] In summary, the embodiments of the present invention achieve constraint satisfaction, disturbance attenuation, and controller performance optimization, effectively calculating the feasible region of equivalent states and control inputs, ensuring robust constraint satisfaction, recursive feasibility, and closed-loop stability, and enabling the robotic arm system to achieve excellent angular position and angular velocity tracking performance. The present invention can achieve high-precision tracking control with excellent disturbance attenuation and constraint satisfaction within the disturbance tolerance range.

[0189] Those skilled in the art will appreciate that the accompanying drawings are merely schematic diagrams of an embodiment, and the modules or processes in the accompanying drawings are not necessarily required to implement the present invention.

[0190] From the above description of the embodiments, it can be seen that those skilled in the art can clearly understand that the present invention can be implemented by means of software plus the necessary general-purpose hardware platform. Based on this understanding, the technical solution of the present invention, or the portion that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a storage medium such as ROM / RAM, a magnetic disk, or an optical disk, and includes a number of instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in various embodiments of the present invention or certain parts of the embodiments.

[0191] Each embodiment in this specification is described in a progressive manner. The same or similar parts between the embodiments can be referred to each other. Each embodiment focuses on the differences from other embodiments. In particular, for the device or system embodiments, since they are basically similar to the method embodiments, the description is relatively simple. For the relevant parts, refer to the partial description of the method embodiments. The device and system embodiments described above are merely schematic, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the scheme of this embodiment. A person of ordinary skill in the art can understand and implement it without making any creative efforts.

[0192] The above description is merely a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in the present invention should be included in the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be based on the scope of protection of the claims.

Claims

1. A joint space trajectory tracking control method for a serial robot arm, characterized in that: include: The original nonlinear multi-input multi-output (MIMO) tracking error system is simplified into a set of decoupled linearized single-input single-output (SISO) tracking error systems using the inner loop inverse dynamics controller, and the set of decoupled linearized single-input single-output (SISO) tracking error systems is used as the inverse dynamics control input. A predictive tracking control (D-RMPTC) scheme for a robust model of a serial manipulator based on a disturbance observer is designed. The D-RMPTC scheme is used to solve a constrained optimization problem and obtain the additive sum of the disturbance compensation input and the prediction input. The inverse dynamics control input, the interference compensation input, and the prediction input are additively formed into the joint torque of the n-degree-of-freedom serial rigid manipulator system, the joint torque of the n-degree-of-freedom serial rigid manipulator system is used to establish the dynamic equation of the n-degree-of-freedom serial rigid manipulator system using the Lagrange equation, and the real-time angle information of the manipulator joint is obtained by solving the dynamic equation of the n-degree-of-freedom serial rigid manipulator system; The inverse dynamics controller using the inner loop simplifies the original nonlinear multi-input multi-output (MIMO) tracking error system into a set of decoupled linearized single-input single-output (SISO) tracking error systems, and uses the set of decoupled linearized single-input single-output (SISO) tracking error systems as inverse dynamics control input, including: The unperturbed reference trajectory is generated by the second-order integrator dynamics: in, is the reference state, is τ ref ∈T reference control input; The state space form of the dynamic equation of the above series rigid manipulator system is: in It indicates the status. It is defined as the equivalent lumped perturbation including uncertainty; is the possible external disturbance torque acting on the joint; By subtracting (1) from (2), we get the tracking error dynamics in the state space: in is the tracking error state, that is: x i =x i,ref +x i,e ,i∈Ι 1:2 ; Using an inverse dynamics controller, the MIMO system is reduced to n SISO decoupled linearized systems, one for each joint of the rigid manipulator system in series, and the control input τ is chosen as: τ=M m (x 1,e +x 1,ref )(u e +u ref )+N m (x 1,e +x 1,ref ,x 2,e +x 2,ref ) (4) in As the auxiliary control input, substitute (4) into (3) to obtain: The tracking error system of the j-th joint is obtained as: The tracking error system is written in a compact form as a matrix: in: B＝[01] T The control input is Given, where u j,edob and u j,empc are generated by the disturbance observer and D-RMPTC controller respectively; The design is based on a predictive tracking control (D-RMPTC) scheme for a robust model of a serial manipulator with a disturbance observer. The D-RMPTC scheme is used to solve the constrained optimization problem to obtain the additive sum of the disturbance compensation input and the prediction input, including: u j,edob Design: Design disturbance observer equivalent lumped disturbance ω j : Among them L j is the observation gain selected by the designer; j is an auxiliary variable; ω j is the equivalent lumped disturbance estimate, and its initial value is set to ω j (t0) = 0; The equivalent lumped disturbance estimation error is defined as Converted into the kinetic equation: For the matching disturbance in the system, the disturbance compensation is designed as follows: The closed-loop system is obtained as: u j,empc Design: Suppressing Lumped Perturbation Estimation Error ω Using D-RMPTC j,e To achieve controller performance, apply standard MPTC to a nominal system with tight state and control constraints, starting from the nominal system with a perturbed system: in and are the nominal state and control input, respectively; The event sampling interval is δ (δ = t k+1 -t k ,t k >t0) indicates the sampling event (x j,e (t k ),t k ) at the nominal finite-horizon optimization problem: in It's about Minimize the quadratic cost function, is the stage cost function, is the terminal cost function, tightening the state constraint set Control constraint set and the terminal state constraint set They are defined as: The optimal control input trajectory and its corresponding optimal predicted state trajectory are obtained by solving the optimization problem.

2. The method according to claim 1, characterized in that The method further comprises: The constraints in the constrained optimization problem include: For a nominal system, there exists Q j ,R j And make A+Bk f Hurwitz stable feedback gain k f Make For control input is a control invariant set, the inequality Applicable to any in According to the disturbance observer dynamics and the disturbance estimation error dynamics, we can obtain: If the system controlled by the control input and the system controlled by the control input The nominal system of control evolves from the same initial state values, i.e. In the time interval [t k ,t k +T], the deviation between the actual state and the nominal best predicted state trajectory satisfies:

3. The method according to claim 2, characterized in that The inverse dynamics control input, the disturbance compensation input, and the prediction input are additively formed into the joint torque of the n-degree-of-freedom serial rigid manipulator system, and the dynamic equation of the n-degree-of-freedom serial rigid manipulator system is established using the Lagrange equation using the joint torque of the n-degree-of-freedom serial rigid manipulator system, including: The inverse dynamics control input, the disturbance compensation input, and the prediction input are additively formed into the joint torque of the n-DOF serial rigid manipulator system. The dynamic equation of the n-DOF serial rigid manipulator system is written in the configuration space using the Lagrange equation using the joint torque of the n-DOF serial rigid manipulator system: in, and are the angular position, velocity and acceleration of the joint, is the joint space inertia matrix, represents the centripetal force matrix and Coriolis moment, represents the gravitational torque, is Coulomb friction and viscous damping, is the joint control torque, is the possible external disturbance torque acting on the joint; The dynamic equations of the serial rigid manipulator system are expressed as follows: Among them, M m (θ) and It is calculated based on the actual measured mechanical dynamic parameters, which are M(θ) and The estimated values ​​of , ΔM(θ) and is the corresponding additional uncertainty; The dynamic equations of the serial rigid manipulator system are rewritten into the following state space form: in It indicates the status. Defined as an equivalent lumped perturbation containing uncertainty, the constraints given by the system are: Where n is the number of joints of the serial manipulator, χ, T and W are convex and compact sets, η 1j ,η 2j >0 is a constant estimated from experimental trials, It is the joint motion limit parameter of a specific robot arm.

4. The method according to claim 3, characterized in that The method of obtaining real-time angle information of the joints of the manipulator by solving the dynamic equations of the n-degree-of-freedom serial rigid manipulator system includes: Based on the Euler forward numerical integration method, the joint angle information of the manipulator joint at the next moment is calculated by solving the dynamic equation of the n-degree-of-freedom series rigid manipulator system according to the input joint torque.

Images