A three-degree-of-freedom robot arm optimal control rate design method based on model prediction static planning

Abstract

The application discloses a kind of three degrees of freedom mechanical arm optimal control rate design methods based on model prediction static programming, guarantee that mechanical arm can be optimized to deal with energy consumption while realizing high-precision trajectory planning optimal control.MPSP (Model Predictive Static Programming) has the advantages of clear controller derivation design process, fixed and high calculation efficiency when solving a kind of optimal control problem with terminal constraint.Through the design of MPSP control algorithm, the mechanical arm can be maneuvered to the desired configuration with high terminal accuracy within a fixed time.In the movement of mechanical arm, the energy consumption optimization target is realized, and energy optimal control is realized.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of mechanical arm control and relates to a kind of finite time domain model prediction static planning optimal control method with terminal hard constraint. BACKGROUND

[0002] With the development of artificial intelligence, people's control requirements for mechanical arms are not only to control them to complete the corresponding tasks. Moreover, on the basis of completing the task within a specified time, it is required to minimize energy consumption and achieve energy saving and emission reduction. Therefore, the idea of optimal control is proposed and used. For the optimal control problem of mechanical arms, there have been a large number of researches. Saridis, George N., in relation to the approximate optimal control of a seven-degree-of-freedom trainable mechanical arm, proposed a recursive algorithm that converges to the optimum [1]. This method, based on the monotonicity between the Hamiltonian variation and the value function proposed by Ljapunov, provides a method for selecting effective control for nonlinear systems. Sun, Ma, et al. proposed a combined control method based on adaptive sliding mode control and optimal control to solve the problems of trajectory tracking and vibration suppression of flexible mechanical arms [2]. Adaptive sliding mode control is used to achieve trajectory tracking of flexible mechanical arms, and linear quadratic optimal control is designed based on the fast time scale model. Zhou, Haibo, proposed an orthogonal fuzzy PID intelligent control method [3], which can further improve the accuracy of the system compared with fuzzy PID. However, the above methods have problems such as overshoot and jitter, which restrict the control accuracy.

[0003] Considering the problems of the above methods, the model predictive control (MPC) method is proposed. MPC can repeatedly optimize and feedback correct the prediction model, and has advantages in mechanical arm control. Zhang, Li, et al. proposed a mechanical arm trajectory tracking model predictive control method based on terminal constraints for the problem of trajectory tracking control of mechanical arms. By setting the objective function and constraint conditions, the trajectory tracking problem is converted into a numerical programming problem of model predictive control [6]. Yuan, W. studied the overall control of a nonholonomic mobile robot using model predictive control and fuzzy logic systems (FLS). A WBC strategy for mobile robots based on model predictive control (MPC) was proposed. This strategy can provide optimal control inputs with specified optimization indicators and constraint conditions [7]. Kleff, Sébastien., first implemented a closed-loop nonlinear MPC on a 7-DOF torque-controlled robot based on model predictive control (MPC). And used an optimal control solver, i.e. differential dynamic programming (DDP), to re-plan state and control trajectories at real-time rates (1 kHz), which is superior to open-loop MPC [8].

[0004] However, the above methods have the following problems: first, the MPC-based control method is proposed for nonlinear systems, which has limitations in application scope; second, the above methods do not solve the mechanical arm energy optimization problem when considering optimal control. In actual engineering applications, the mechanical arm carries limited energy, and to ensure the endurance performance of the mechanical arm, energy optimization must be considered; third, the above control methods do not consider the constraints of the mechanical arm terminal state, and have large calculation amount, complex calculation and low control efficiency.

[0005] The relevant references are as follows:

[0006] [1]Saridis,George N.,and Chun-Sing G.Lee."An approximation theory of optimal control for trainable manipulators."IEEE Transactions on systems,Man,and Cybernetics 9,no.3(1979):152-159.

[0007] [2]Sun Xiaotong,Lei Ma,Mengxue Li,Qichao Tang,Tianlong Hou. Double-time scale combined control of flexible manipulator based on adaptive sliding mode-optimal control[J]. Noise and vibration control,2020,40(05):22-26+45.

[0008] [3]Zhou,Haibo,Rui Chen,Shun Zhou,and Zhenzhong Liu."Design and analysis of a drive system for a series manipulator based on orthogonal-fuzzy PID control."Electronics 8,no.9(2019):1051.

[0009] [4]Zhu Zhuangang,Guo Ruiqin,Wu Shun,Qiang Yu,Zhao Qingyan,Tang Qirong. Review of model predictive control method in manipulator operation[J]. Automation technology and application,2021,40(11):1-8.

[0010] [5]Ni Qi. Hierarchical structure control strategy of six-degree-of-freedom manipulator based on MPC[D]. Xi'an University of Science and Technology,2020.DOI:10.27397 / d.cnki.gxaku.2020.000133.

[0011] [6]Zhang, L., Li, Y., Wang, X., & Wang, W. (2022). Manipulator Terminal Constraint Model Predictive Control Method under Co-simulation. Journal of Xi'an University of Engineering Science, 36(04), 118-126. 2022.04.017.

[0012] [7]Yuan, W., Liu, Y. H., Su, C. Y., & Zhao, F. (2022). Whole-Body Control of an Autonomous Mobile Manipulator Using Model Predictive Control and Adaptive Fuzzy Technique. IEEE Transactions on Fuzzy Systems.

[0013] [8]Kleff, S., Meduri, A., Budhiraja, R., Mansard, N., & Righetti, L. (2021). High-frequency nonlinear model predictive control of a manipulator. In 2021 IEEE International Conference on Robotics and Automation (ICRA), 7330-7336. IEEE. SUMMARY

[0014] In order to overcome the shortcomings of the prior art, the present application proposes a model predictive static planning control method, comprising the following steps:

[0015] Step 1, constructing a three-degree-of-freedom manipulator dynamics model;

[0016] Step 2, deriving the continuous dynamics equation of infinitesimal change based on the dynamics model;

[0017] Step 3, solving the discrete dynamics equation according to the continuous dynamics equation of infinitesimal change;

[0018] Step 4, iteratively solving the change of control rate based on the discrete dynamics equation, and realizing the design of the control rate of the three-degree-of-freedom manipulator based on the model predictive static planning method;

[0019] Specifically, the method comprises the following steps: firstly, initializing a control rate sequence; substituting the initialized control rate sequence into a discrete dynamic equation to obtain a state quantity and an output quantity; obtaining an error according to the output quantity and an expected value; then, judging whether the error is less than a threshold value; if the error is greater than the threshold value, calculating a sensitivity matrix, and re-calculating the control rate sequence according to the sensitivity matrix, substituting the control rate sequence into the discrete dynamic equation, and re-iterating to obtain the state quantity and the output quantity, and further obtaining the error according to the expected value, until the error is less than the threshold value, and outputting the control rate sequence.

[0020] Further, the three-degree-of-freedom mechanical arm dynamic model constructed in step 1 is as follows:

[0021]

[0022] wherein, is an inertia matrix of each joint of the three-degree-of-freedom mechanical arm, is a centrifugal force and Coriolis force matrix, is an angular displacement of each joint of the three-degree-of-freedom mechanical arm, q = [θ1, θ2, θ3] T , is an angular velocity of each joint of the three-degree-of-freedom mechanical arm, is an angular acceleration of each joint of the three-degree-of-freedom mechanical arm, is a motor control torque, is an error and disturbance.

[0023] Further, the specific implementation mode of step 2 is as follows:

[0024] According to formula (1), the dynamic model of the mechanical arm with three degrees of freedom can be transformed into:

[0025]

[0026] Let X represent the state quantity, Y represent the output quantity, and U represent the control rate, when the output quantity is equal to the state quantity, then:

[0027]

[0028]

[0029]

[0030] The infinitesimal change of X, Y and U is dX, dY and dU;

[0031]

[0032]

[0033]

[0034] Based on equation (2) and the variational principle, the infinitesimal variation of is

[0035]

[0036]

[0037] The derivative of dX with respect to time is :

[0038]

[0039] where I3and 0 3*3 The specific form of I3is as follows:

[0040]

[0041]

[0042] Further, the specific implementation of step 3 is as follows:

[0043] Based on Euler integration, the discrete dynamic form of equation (11) is

[0044]

[0045] In the above equation, h is the discrete step size, k is the discrete time, matrix B k is the sensitivity matrix, represents the discrete variable sampled at time k, matrix A' k , B' k is:

[0046]

[0047] Further, the specific implementation of step 4 is as follows:

[0048] The control rate of a three-degree-of-freedom robot arm is:

[0049]

[0050] where is a positive definite weight matrix, matrix B k is the sensitivity matrix;

[0051]

[0052] is the control input sequence obtained in the last iteration, dY N is the terminal error, defined as

[0053]

[0054] Among them, e qN =q N -q d Let q be the state variable q at terminal time N. N With expected value q d The error, for State quantity at time N in the terminal Compared with expected value The error.

[0055] Furthermore, in step 4, the control rate sequence is initialized to 0.

[0056] Furthermore, the formula for calculating the sensitivity matrix is ​​as follows:

[0057]

[0058] Where k = 1, 2, ..., N are discrete time points. Let F be a state vector, and both F and h be known function expressions. These are the input vector and the output vector, respectively, and matrix B. k Let be the sensitivity matrix, which can be obtained by recursively solving the following equation:

[0059]

[0060] Compared with the prior art, the beneficial effects of the present invention are:

[0061] (1) Not only does it consider the precise control of the robotic arm with hard constraints at the end, but it also achieves optimal energy consumption.

[0062] (2) The derivation and design process of the controller is clear, fixed, and computationally efficient;

[0063] (3) Wide range of applications. Attached Figure Description

[0064] Figure 1 This is a flowchart of the method of the present invention. Detailed Implementation

[0065] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and specific implementation steps.

[0066] First, we derive a model predictive static programming algorithm based on the design of general explicit discrete dynamic systems in Euclidean space.

[0067] Consider a general explicit discrete dynamical system in Euclidean space, whose state equations and output equations are:

[0068] X k+1 = F k (X k , U k ) (1)

[0069] Y k = h k (X k ) (2)

[0070] where, is the state vector, F k and h k are function expressions. are the input vector and output vector respectively. k = 1, 2,..., N are discrete time points. The objective of the optimization algorithm is to find the appropriate control input sequence U k , k = 1, 2,..., (N-1) such that the explicit discrete dynamic system achieves the terminal output Y N equal to the desired output Y d , i.e. Y N → Y d .

[0071] Since Y N and Y d belong to the same flat Euclidean space Y N and Y d , the error between Y N and Y N can be calculated as ΔY d = Y N - Y N . When ||ΔY d || is small, Y N can be approximated by the Taylor expansion of (2) at Y k . Ignoring the higher order terms of the Taylor expansion, ΔY N can be expressed as

[0072]

[0073] From (2), the state error at k+1 time instant can be expressed by the state error at k time instant and the input error as:

[0074]

[0075] Combining dX k in (4), dY N in (4) can be expanded as

[0076]

[0077] Similarly, dX k in (4) can be expanded asN-1 It can be described by the state error and input error at time N-2. Repeat this step until k=1, dY N It can be expanded into

[0078] dY N =AdX1+B1dU1+B2dU2+…+B N-1 dU N-1 (6)

[0079] In the formula The expression is

[0080]

[0081] Wherein, matrix B k Let be the sensitivity matrix, which can be obtained by recursively solving the following equation:

[0082]

[0083] Since the initial state X1 of the system is fixed, i.e., dX1 = 0, equation (6) can be further simplified to:

[0084]

[0085] Equation (9) contains (N-1)m unknowns and p linear equality constraints. Generally, p < (N-1)m, therefore equation (9) has multiple solutions. To ensure that the solution to the system is unique, additional constraints or requirements need to be introduced, such as minimizing a certain objective (cost) function, for example:

[0086]

[0087] In the formula, This is the control input sequence obtained from the previous iteration. dU k for The correction amount. This refers to the control variable updated in this iteration, and is independent of the state variables and control inputs from previous time steps. The weight matrix is ​​positive definite. Therefore, equations (9) and (10) constitute a linear programming problem. Based on optimal control theory, the unique solution to equation (9) is:

[0088]

[0089] in

[0090]

[0091] Thus, the derivation of the model prediction static programming algorithm based on the general Euclidean space dynamics model is complete.

[0092] Next, based on the above derivation process, the optimal control rate method of a three-degree-of-freedom robot arm based on model predictive static planning is designed.

[0093] Step 1, construct the dynamic model of the three-degree-of-freedom robot arm as follows:

[0094]

[0095] wherein, is the inertia matrix of each joint of the three-degree-of-freedom robot arm, is the centrifugal force and Coriolis force matrix, is the angular displacement of each joint of the three-degree-of-freedom robot arm, is the angular velocity of each joint of the three-degree-of-freedom robot arm, is the angular acceleration of each joint of the three-degree-of-freedom robot arm, is the motor control torque, τ = [τ1, τ2, τ3] T , is the error and disturbance.

[0096] Step 2, based on the dynamic model, the continuous dynamics equation of the infinitesimal change is derived;

[0097] The dynamic model of the three-degree-of-freedom robot arm can be converted to:

[0098]

[0099] Let X represent the state quantity, Y represent the output quantity, and U represent the control rate, then

[0100]

[0101]

[0102]

[0103] From equation (13), based on the variational principle, the infinitesimal change of is

[0104]

[0105]

[0106] The infinitesimal change dX of the state quantity X can be expressed as

[0107]

[0108] Similarly, dY and dU can be expressed as

[0109]

[0110]

[0111] The derivative of dX with respect to time from equation (17), (18) is arranged as

[0112]

[0113] where I3 and 0 3*3 The specific form is as follows:

[0114]

[0115]

[0116] Equation (22) constitutes a continuous dynamic system with dX as the state variable and dU as the input variable. In order to facilitate the design and application of the MPSP algorithm, equation (22) needs to be converted into a discrete dynamic form.

[0117] Step 3, the discrete dynamic equation is obtained according to the continuous dynamic equation of infinitesimal change;

[0118] Based on Euler integration, the discrete dynamic form of equation (22) is obtained as

[0119]

[0120] In the above equation, h is the discrete step size, k = 1, 2, 3,... N is the discrete time, represents the discrete variable sampled at time k. The matrix A' k , B' k is

[0121]

[0122] Step 4, the change of control rate is iteratively calculated based on the discrete dynamic equation, and the control rate of the three-degree-of-freedom manipulator is designed based on the model predictive static programming method;

[0123] Specifically, first, the control rate sequence is initialized, and the initialized control rate sequence is substituted into the discrete dynamic equation to calculate the state variable (state variable X, i.e. the q and The error is calculated according to the output quantity and the expected value, and then it is determined whether the error is less than a threshold value. If the error is greater than the threshold value, the sensitivity matrix is calculated, and the control rate sequence is recalculated according to the sensitivity matrix, substituted into the discrete dynamic equation, and iterated to obtain the state quantity and the output quantity. The error is further calculated according to the output quantity and the expected value, until the error is less than the threshold value, and the control rate sequence is output.

[0124] First, the control sequence is initialized to 0, that is, the initial control input is 0.

[0125] Then, based on formula (23) and formula (24), the model predictive static planning algorithm control rate of the 3-DOF robot arm is as follows:

[0126]

[0127] where dY N is the terminal prediction error between Y and Y . is obtained by numerical integration iteration of formula (13), which is the terminal predicted pose of the robot arm. is the expected pose of the robot arm. As can be seen from formula (15), dY N includes the terminal pose error and the terminal velocity error .Therefore, there is

[0128]

[0129] where, is the error of the state quantity q N of q at the terminal N time and the expected value q d . is the error of the state quantity at the terminal N time and the expected value .

[0130] Under the control rate input, the robot arm can reach the expected state within the terminal output time on the basis of considering the energy consumption optimization.

[0131] The specific embodiments described herein are merely illustrative of the spirit of the present application. Those skilled in the art to which the present application belongs can make various modifications or supplements to the described specific embodiments or replace them with similar ways, without departing from the spirit of the present application or exceeding the scope defined by the appended claims.

Claims

1. A method for designing the optimal control law of a three-degree-of-freedom robotic arm based on model predictive static programming, characterized in that, Includes the following steps: Step 1: Construct a dynamic model of a three-degree-of-freedom robotic arm; The three-degree-of-freedom robotic arm dynamic model constructed in step 1 is as follows: in, Here are the inertia matrices of each joint of the three-degree-of-freedom robotic arm. The matrix represents the centrifugal force and the Coriolis force. Let q represent the angular displacements of each joint of the three-degree-of-freedom robotic arm, where q = [θ1, θ2, θ3]. T , Let be the angular velocities of each joint of the three-degree-of-freedom robotic arm. For the angular accelerations of each joint of the three-degree-of-freedom robotic arm, The motor control torque is τ = [τ1, τ2, τ3]. T , For errors and disturbances; Step 2: Derive the continuous dynamic equations for infinitesimal changes based on the dynamic model; The specific implementation method of step 2 is as follows; From equation (1), the dynamic model of the robotic arm with three degrees of freedom can be transformed into: Let X represent the state variable, Y represent the output variable, and U represent the control law. When the output variable equals the state variable, then: The infinitesimal changes of X, Y, and U are dX, dY, and dU, respectively. Based on equation (2) and the variational principle, we can obtain The infinitesimal change is: Therefore, the derivative of dX with respect to time can be obtained. for: Among them, I3 and 0 3*3 The specific form is as follows: Step 3: Obtain the discrete dynamic equations based on the continuous dynamic equations of infinitesimal changes; The specific implementation method of step 3 is as follows; Based on the Euler integral, the discrete dynamic form of equation (11) can be obtained as follows: In the above formula, h is the discrete step size, k is the discrete time, and matrix B k For the sensitivity matrix, express The discrete variables sampled at time k, matrix A' k B' k for: Step 4: Iteratively calculate the change in control rate based on discrete dynamic equations, and design the control rate of the three-degree-of-freedom robotic arm based on model prediction static programming method; Specifically, the process includes: first, initializing the control rate sequence; substituting the initial control rate sequence into the discrete dynamics equation to obtain the state variables and output variables; calculating the error based on the output variables and the expected value; then determining whether the error is less than a threshold; if the error is greater than the threshold, calculating the sensitivity matrix; and recalculating the control rate sequence based on the sensitivity matrix, substituting it into the discrete dynamics equation, iterating again to obtain the state variables and output variables, and then calculating the error again based on the expected value, until the error is less than the threshold, and finally outputting the control rate sequence.

2. The optimal control law design method for a three-degree-of-freedom robotic arm based on model predictive static programming as described in claim 1, characterized in that: The specific implementation method of step 4 is as follows; The control rate of the three-degree-of-freedom robotic arm is: in Let B be a positive definite weight matrix. k This is the sensitivity matrix; Given the control input sequence dY obtained in the previous iteration, N Terminal error is defined as follows: in, Let q be the state variable q at terminal time N. N With expected value q d The error, for State quantity at time N in the terminal Compared with expected value The error.

3. The optimal control law design method for a three-degree-of-freedom robotic arm based on model predictive static programming as described in claim 1, characterized in that: In step 4, the control rate sequence is initialized to 0.

4. The optimal control law design method for a three-degree-of-freedom robotic arm based on model predictive static programming as described in claim 2, characterized in that: The formula for calculating the sensitivity matrix is ​​as follows: Where k = 1, 2, ..., N are discrete time points. Let F be a state vector, and both F and h be known function expressions. These are the input vector and the output vector, respectively, and matrix B. k Let be the sensitivity matrix, which can be obtained by recursively solving the following equation:

Images