Motion control method for rope-driven mechanical arm based on model predictive control

Abstract

The invention discloses a rope-driven mechanical arm motion control method based on model predictive control, which comprises the following steps: taking a joint angle error as a state variable, taking a rope-driven rotation angle as an input variable, and establishing a nonlinear state-space equation of a rope-driven mechanical arm according to the state variable and the input variable; the nonlinear state-space equation is subjected to linearization processing, a linear approximation model is generated, and the linear approximation model comprises a linearization matrix; defining a prediction interval, expanding the linear approximation model into a multi-step prediction form, and generating a prediction model; constructing a cost function, and combining the prediction model into the cost function to solve and obtain an optimal control sequence; and extracting the optimal control increment at the current moment from the optimal control sequence, and updating the state variable, the input variable and the linearization matrix according to the optimal control increment to form closed-loop control. Accurate path tracking control can be carried out on the rope-driven mechanical arm under complex dynamic constraints, and it is ensured that the mechanical arm moves according to the planned trajectory.

Description

Technical Field

[0001] The present invention relates to the technical field of robotic arm control, and in particular, to a motion control method for a cable-driven robotic arm based on model predictive control. Background Art

[0002] With the continuous development of robot industry technology, and the characteristics of robots such as low cost, easy deployment and high flexibility, robots have gradually been taken seriously, and the industry scale has also increased rapidly. Currently, most robots use traditional industrial robotic arms, which have the characteristics of large joint mass, large inertia and poor compliance. A motor and a reducer are directly installed at each joint for driving, resulting in a large joint mass and an increase in inertia. During operation, the lower joints must overcome the influence of the motors of the upper joints to do work, resulting in a reduction in motion efficiency. Due to the high structural rigidity, traditional robotic arms are difficult to meet the requirements when flexible adjustment is needed, and cannot quickly adapt to changes in the external environment and emergencies. When performing a grasping task, the large mass and inertia of the robotic arm will cause more energy waste, especially when overcoming inertia and reducer friction, further reducing the energy utilization efficiency.

[0003] The introduction of cable drive technology provides a new idea for solving the above problems. A cable-driven robotic arm is a robotic arm that uses a cable system to achieve motion control and drive. By placing the motor and the drive device at the base position, the end of the cable-driven robotic arm only needs to carry a light transmission device and cables, significantly reducing the total mass and inertia of the robotic arm, making the robotic arm easier to move and control, and improving the motion flexibility. By controlling the pulling and relaxation of the cables, the motion and posture adjustment of the robotic arm can be achieved. It can better adapt to the complex and changing environmental requirements during task execution, and provide more precise control and posture adjustment capabilities.

[0004] However, the cable itself has elastic deformation characteristics, and unpredictable stretching and deformation may occur due to tension changes during operation. Nonlinear factors such as cable friction and slack also affect the transmission accuracy, making the dynamic performance of the system more complex and difficult to accurately model and control. Due to the flexibility and dynamic characteristics of the cables, the structural parameters of the cable-driven robotic arm are prone to change, making the modeling process complex and with great uncertainty. This parameter change may affect the control accuracy and increase the uncertainty and complexity in motion control. In addition, compared with a robotic arm with rigid connections, the transmission model of a cable-driven robotic arm involves multiple couplings and nonlinear effects, significantly increasing the difficulty of describing and solving the dynamic model, and bringing greater challenges to path planning and precise control.

[0005] Therefore, how to accurately perform path tracking control on a cable-driven robotic arm under these complex dynamic constraints to ensure that the robotic arm moves along the planned trajectory is an important technical problem.

[0006] The disclosure of the above background art content is only for assisting in understanding the concept and technical solution of the present invention, and it does not necessarily belong to the prior art of this patent application. Without clear evidence indicating that the above content was publicly available on the filing date of this patent application, the above background art should not be used to evaluate the novelty and inventiveness of this application. Summary of the Invention

[0007] To solve the above technical problems, the present invention proposes a motion control method for a cable-driven manipulator based on model predictive control, which can accurately perform path tracking control on the cable-driven manipulator under complex dynamic constraints to ensure that the manipulator moves along the planned trajectory.

[0008] To achieve the above object, the present invention adopts the following technical solutions:

[0009] In a first aspect, the present invention discloses a motion control method for a cable-driven manipulator based on model predictive control, including the following steps:

[0010] S1: Taking the joint angle error as the state variable and the cable drive rotation angle as the input variable, establish a nonlinear state space equation of the cable-driven manipulator according to the state variable and the input variable;

[0011] S2: Linearize the nonlinear state space equation to generate a linear approximation model, and the linear approximation model includes a linearization matrix;

[0012] S3: Define a prediction interval, expand the linear approximation model into a multi-step prediction form to generate a prediction model;

[0013] S4: Construct a cost function, combine the prediction model into the cost function to solve for the optimal control sequence;

[0014] S5: Extract the optimal control increment at the current moment from the optimal control sequence, and update the state variable, input variable, and linearization matrix according to the optimal control increment to form a closed-loop control.

[0015] Preferably, the nonlinear state space equation of the cable-driven manipulator established according to the state variable and the input variable in step S1 is:

[0016] x(k + 1) = x(k) + g{u(k)} - θ d (k + 1)

[0017] where x(k) represents the state variable at time k, u(k) represents the input variable at time k, g{} represents the kinematic relationship between the joint angle and the cable drive rotation angle, and θ d (k + 1) represents the desired joint angle at time k + 1.

[0018] Preferably, in step S2, the nonlinear state - space equation is linearized by Taylor series expansion, and the generated linear approximation model is:

[0019]

[0020] where x(k) represents the state variable at time k, Δu(k)=u(k) - u * (k - 1), u(k) represents the input variable at time k, u * (k - 1) represents the optimal control input at time k - 1, A represents the identity matrix, B represents the linearization matrix, and D represents the constant compensation term.

[0021] Preferably, the linearization parameter B is obtained by solving the matrix partial derivative at the equilibrium point according to the first - order approximation of the Taylor series expansion, where the equilibrium point includes the state variable and input variable corresponding to the optimal control input at the previous time.

[0022] Preferably, the prediction model generated in step S3 is:

[0023] X k = Mx(k)+CU k + ND

[0024] M = [I,A,A 2 ,…,A N T

[0025]

[0026] where X k represents the predicted value of the state variable at time k, x(k) represents the state variable at time k, U k represents the predicted value of the input variable at time k, N is the prediction horizon, D represents the constant compensation term, M is an (N + 1)n×n matrix, C is an (N + 1)n×Np matrix, N is the prediction horizon, n, p represent the degrees of freedom of the cable - driven robotic arm, A represents the identity matrix, and B represents the linearization matrix.

[0027] Preferably, the cost function constructed in step S4 is:

[0028]

[0029] where J is the cost function, X k represents the predicted value of the state variable at time k, U k represents the predicted value of the input variable at time k, F represents the weighting matrix at the prediction end - point, Q represents the weighting matrix at the prediction process points, and R represents the control weighting matrix.

[0030] ​Preferably, in step S4, combining the prediction model into the cost function to solve for the optimal control sequence specifically includes: designing the optimization problem of the cost function as a quadratic programming problem that includes the control joint angle error and the smoothness of the control input variable, and solving the quadratic programming problem through an optimization solver to obtain the optimal control sequence at the current moment.

[0031] Preferably, forming a closed-loop control in step S5 means repeatedly executing steps S1 to S5 until the cable-driven robotic arm completes the trajectory movement along the desired trajectory.

[0032] Preferably, the cable-driven robotic arm has a 7-degree-of-freedom redundant configuration and is composed of a two-degree-of-freedom shoulder direct drive joint, a two-degree-of-freedom elbow separated tension amplification mechanism, and a three-degree-of-freedom wrist parallel-universal joint composite drive mechanism connected in series.

[0033] In a second aspect, the present invention discloses a computer-readable storage medium in which a computer program is stored. Wherein, the computer program is configured to be run by a processor to execute the motion control method of the cable-driven robotic arm described in the first aspect.

[0034] Compared with the prior art, the beneficial effects of the present invention are as follows: The motion control method of the cable-driven robotic arm based on model predictive control disclosed in the present invention establishes a non-linear state equation of the cable-driven robotic arm by using the joint angle error and the cable-driven rotation angle as the state variable and the input variable respectively, and performs linearization processing. Then, the linear approximation model is extended to a multi-step prediction form of the prediction model to achieve model predictive control, effectively coping with the system non-linearity and coupling problems, realizing precise control and system optimization of complex trajectories. Therefore, it can effectively solve the problem that in a multi-input multi-output system, the coupling relationship between joints makes the parameter debugging of traditional motion control methods very complicated, and it is difficult to ensure the control performance indexes of each joint at the same time; thus, it can further perform accurate path tracking control on the cable-driven robotic arm under complex dynamic constraints to ensure that the robotic arm moves along the planned trajectory. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 is a flowchart of the motion control method of the cable-driven robotic arm based on model predictive control disclosed in the preferred embodiment of the present invention;

[0036] Figure 2a is a structural schematic diagram of the cable-driven robotic arm in a specific embodiment of the present invention;

[0037] Figure 2b is Figure 2a a schematic diagram of the cable connection between the wrist joint and the elbow joint of the cable-driven robotic arm in

[0038] Figure 2c is Figure 2aSchematic diagram of the cable connection between the elbow joint and the shoulder joint of the cable-driven manipulator;

[0039] Figure 3 It is a basic concept diagram of model predictive control;

[0040] Figure 4 It is a flowchart of the motion control algorithm of the specific cable-driven manipulator based on MPC control;

[0041] Figure 5a It is a comparison schematic diagram of the target trajectory and the actual trajectory of the joint angle in the arc part;

[0042] Figure 5b It is a comparison schematic diagram of the target trajectory and the actual trajectory of the joint angle in the straight line part;

[0043] Figure 6a It is a comparison schematic diagram of the target trajectory and the actual trajectory of the cable-driven rotation angle in the arc part;

[0044] Figure 6b It is a comparison schematic diagram of the target trajectory and the actual trajectory of the cable-driven rotation angle in the straight line part;

[0045] Figure 7a It is a comparison schematic diagram of the target trajectory and the actual trajectory of the change values of the end XYZ coordinates at each moment in the arc part;

[0046] Figure 7b It is a comparison schematic diagram of the target trajectory and the actual trajectory of the change values of the end XYZ coordinates at each moment in the straight line part. Detailed implementation mode

[0047] The following provides a detailed description of the implementation mode of the present invention. It should be emphasized that the following description is merely exemplary and is not intended to limit the scope of the present invention and its applications.

[0048] It should be noted that when an element is referred to as "fixed to" or "disposed on" another element, it can be directly on the other element or indirectly on that other element. When an element is referred to as "connected to" another element, it can be directly connected to the other element or indirectly connected to that other element. Additionally, the connection can be for a fixing function or for a circuit / signal connection function.

[0049] It should be understood that the orientation or positional relationship indicated by the terms "length", "width", "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", etc. is based on the orientation or positional relationship shown in the drawings, and is only for the convenience of describing the embodiments of the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and thus should not be construed as a limitation of the present invention.

[0050] In addition, the terms "first" and "second" are for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the quantity of the indicated technical features. Thus, features defined with "first" and "second" may explicitly or implicitly include one or more such features. In the description of the embodiments of the present invention, "a plurality of" means two or more unless otherwise specifically defined.

[0051] As Figure 1 shown, a motion control method for a cable-driven robotic arm based on model predictive control is disclosed in a preferred embodiment of the present invention. Model predictive control (MPC) is used to optimize the control of the system to achieve precise trajectory tracking and motion control and overcome the nonlinear effects of the system. The specific steps are as follows:

[0052] S1: Taking the joint angle error as the state variable and the cable-driven rotation angle as the input variable, a nonlinear state space equation of the cable-driven robotic arm is established according to the state variable and the input variable;

[0053] Among them, the cable-driven robotic arm is a 7-degree-of-freedom redundant configuration, which is composed of a two-degree-of-freedom shoulder direct drive joint, a two-degree-of-freedom elbow separated tension amplification mechanism, and a three-degree-of-freedom wrist parallel-universal joint composite transmission mechanism in series.

[0054] Among them, the nonlinear state space equation of the cable-driven robotic arm established according to the state variable and the input variable is:

[0055] x(k + 1) = x(k) + g{u(k)} - θ d (k + 1)

[0056] In the formula, x(k) represents the state variable at time k, uy(k) represents the input variable at time k, g{} represents the kinematic relationship between the joint angle and the cable-driven rotation angle, and θ d (k + 1) represents the desired joint angle at time k + 1.

[0057] S2: Linearize the nonlinear state space equation to generate a linear approximation model, and the linear approximation model includes a linearization matrix;

[0058] Among them, specifically, the nonlinear state space equation is linearized by Taylor series expansion, and the generated linear approximation model is:

[0059]

[0060] In the formula, x(k) represents the state variable at time k, Δu(k) = u(k) - u * (k - 1), u(k) represents the input variable at time k, and u *(k - 1) represents the optimal control input at time k - 1, A represents the identity matrix, B represents the linearization matrix, and D represents the constant compensation term.

[0061] The linearization parameter B is obtained by solving the matrix partial derivative at the equilibrium point based on the first-order approximation of the Taylor series expansion, where the equilibrium point includes the state variables and input variables corresponding to the optimal control input at the previous time.

[0062] In some other embodiments, other non-linear model predictive control methods besides Taylor expansion can also be used to improve and optimize the control strategy.

[0063] S3: Define the prediction interval, expand the linear approximation model into a multi-step prediction form, and generate a prediction model;

[0064] Among them, the generated prediction model is:

[0065] X k = Mx(k) + CU k + ND

[0066] M = [I, A, A 2 ,…, A N T

[0067]

[0068] In the formula, X k represents the predicted value of the state variable at time k, x(k) represents the state variable at time k, U k represents the predicted value of the input variable at time k, N is the prediction interval, D represents the constant compensation term, M is an (N + 1)n×n matrix, C is an (N + 1)n×Np matrix, N is the prediction interval, n and p represent the degrees of freedom of the cable-driven robotic arm, A represents the identity matrix, and B represents the linearization matrix.

[0069] S4: Construct a cost function, combine the prediction model into the cost function to solve for the optimal control sequence;

[0070] Among them, the constructed cost function is:

[0071]

[0072] In the formula, J is the cost function, X k represents the predicted value of the state variable at time k, U k represents the predicted value of the input variable at time k, F represents the weighted matrix at the prediction end point, Q represents the weighted matrix at the prediction process points, and R represents the control weighted matrix.

[0073] ​In step S4, incorporating the prediction model into the cost function to solve for the optimal control sequence specifically includes: designing the optimization problem of the cost function as a quadratic programming problem that includes the control joint angle error and the smoothness of the control input variables, and solving the quadratic programming problem through an optimization solver to obtain the optimal control sequence at the current moment.

[0074] S5: Extract the optimal control increment at the current moment from the optimal control sequence, and update the state variables, input variables, and linearization matrix according to the optimal control increment to form a closed-loop control.

[0075] The optimization control strategy based on MPC in this embodiment is implemented as follows: constructing a prediction model and a cost function, unifying the multi-step control objective and control smoothness into the optimization objective; transforming the control problem into a standard quadratic programming problem (QP), and realizing the online optimal control of the system through iterative solution; only applying the optimal solution of the current step each time, adapting to the dynamic changes of the system, and realizing the real-time online control closed-loop. Among them, forming a closed-loop control means repeatedly executing steps S1 to S5 until the cable-driven robotic arm completes the trajectory movement along the desired trajectory.

[0076] The motion control method of the cable-driven robotic arm based on model predictive control proposed in the preferred embodiment of the present invention solves the following problems:

[0077] First, the precise control problem of multi-input multi-output nonlinear systems. Due to nonlinear factors such as elastic deformation and friction in the cable-driven robotic arm, its dynamic modeling and control are complex. Existing control methods often show insufficient control accuracy and system coupling problems when dealing with complex multi-input multi-output (MIMO) systems. Based on this, the present invention proposes a motion control method of the cable-driven robotic arm based on model predictive control (MPC), effectively coping with system nonlinearity and coupling problems, and realizing precise control of complex trajectories and system optimization.

[0078] Second, the problem of constructing a control model under the structure of a complex redundant cable-driven robotic arm. Due to the redundant degrees of freedom of the cable-driven robotic arm and the nonlinear characteristics of the cables, the establishment of its control model is very complex, involving accurate mapping between the operation space, joint space, and cable drive space. The present invention solves the problems of motion description and calculation of the redundant cable-driven robotic arm in multiple spaces by constructing a control model, providing necessary data support and basic calculation framework for the precise control and trajectory planning of the system.

[0079] The following studies the 7-degree-of-freedom (7-DOF) redundant cable-driven robotic arm in combination with the motion control method of the cable-driven robotic arm based on model predictive control proposed in the preferred embodiment of the present invention, introducing model predictive control into the motion control of the complex redundant cable-driven robotic arm to achieve a high degree of coordination between the structure and the control.

[0080] The overall configuration of the 7-DOF cable-driven robotic arm is asFigures 2a to 2c As shown. The rope-driven manipulator body is a 7-DOF manipulator, which is composed of a two-DOF shoulder, a two-DOF elbow and a three-DOF wrist in series. The shoulder is directly driven by a joint motor, and there is no motion coupling. The elbow joint and the wrist joint realize a total of five degrees of freedom motion, both of which are driven by ropes. The elbow adopts two sets of single-DOF separate tension amplification mechanisms, and the wrist consists of a 2-DOF parallel mechanism and a 1-DOF universal joint rotation mechanism. The drive motor is placed at the rear of the shoulder for remote control. Figure 2a As shown, the rope-driven manipulator includes a first shoulder joint 10, a second shoulder joint 20, a first elbow joint 30, a second elbow joint 40 and a wrist joint 50; Figure 2b As shown, at the first elbow joint 30, the connecting end discs 31 at both ends are connected to each other through the elbow joint intermediate link 32, the elbow joint excitation side rope 33, and the elbow joint antagonism side rope 34; Figure 2c As shown, at the wrist joint 50, the dynamic platform 51 and the static platform 52 are connected in linkage through a universal joint 53, a Yaw direction excitation side rope 54, a Yaw direction antagonistic side rope 55, a Pitch direction excitation side rope 56, a Pitch direction antagonistic side rope 57, a Roll direction excitation side rope 58, and a Roll direction antagonistic side rope 59.

[0081] In the first part, we first introduce the basic principles of model predictive control.

[0082] MPC is usually used for discrete systems, so for continuous systems, discretization must be performed first. The basic concept diagram is as follows Figure 3 As shown. Establish the state equation of the system:

[0083] x(k+1)=Ax(k)+Bu(k) (1)

[0084] Where x is the system state variable and u is the input variable (i.e., control variable). This state equation is applicable to linear systems. To make the state equation more general, the following equation is used:

[0085] x(k+1)=f{x(k),u(k)} (2)

[0086] The performance indicators are defined as

[0087]

[0088] Among them, \(N\) is the prediction interval, \(x(k + i|k)\) is the value of the state variable at time \(k + i\) predicted at time \(k\), \(u(k + i|k)\) is the control strategy calculated at time \(k\) for time \(k + i\), \(g\) represents the stage cost function, and \(h\) represents the terminal cost function. The system starts at time \(k\) with an initial state of \(x(k)\). By solving the optimization problem within the prediction interval, that is, minimizing the performance index \(J\) within the prediction interval \(N\), the optimal control sequence \(u(k|k), u(k + 1|k), \cdots, u(k + N - 1|k)\) is obtained. Then, substituting it into the state equation, the state values \(x(k|k), x(k + 1|k), \cdots, x(k + N|k)\) of the prediction system under this control sequence are solved. After completing the calculation of the control quantity and the prediction model, only \(u(k|k)\) is applied to the system, which is used as the finally solved optimal strategy (i.e., the optimal control input quantity) \(u\). * (k).

[0089] At time \(k + 1\), the operation process at time \(k\) is repeated. At this time, the initial state is \(x(k + 1)\) obtained under the optimal control input quantity \(u\). * (k) control strategy. The prediction interval moves forward by one discrete step length and continuously runs repeatedly online as time progresses. At each new time, calculation and analysis are required. Through the prediction interval, the control sequence \(u(k)\) at the current time is obtained and substituted into the state equation to update the initial state value at the next time.

[0090] The second part introduces the specific implementation of the control algorithm, which specifically includes the following five major steps:

[0091] A1: Establish the state space equation

[0092] Using the model predictive control method to perform motion control on the handling cable-driven manipulator, taking the motor rotation angle as the input variable. Since the motor rotation angle and the cable-driven rotation angle differ by the reduction ratio \(\eta\) coefficient, when considering later, the cable-driven rotation angle \(driq\) is used to replace the motor rotation angle as the input variable \(u\), and the error between the actual joint angle and the desired joint angle, that is, the joint angle error \(e\). q As the state variable, establish the state space expression of the discrete-time system:

[0093] e q (k + 1)=e q (k)+θ c (k)-θ d (k + 1) (4)

[0094] Among them, \(θ\). c Is the current actual joint angle, \(θ\). d Is the known desired joint angle, and the desired joint angle is obtained from the inverse kinematics of the desired trajectory after trajectory planning.

[0095] Moreover, there is a kinematic relationship between the current actual joint angle and the cable-driven rotation angle, i.e.:

[0096] θ c (k) = g{driq(k)} (5)

[0097] The state equation is expressed as

[0098] e q (k + 1) = e q (k) + g{driq(k)} - θ d (k + 1) (6)

[0099] After expressing it in terms of the state variable x and the input variable u

[0100] x(k + 1) = x(k) + g{u(k)} - θ d (k + 1) (7)

[0101] where both the state variable x and the input variable u are vectors, x = [x1, x2, x3, x4, x5, x6, x7] T corresponding to the joint errors of 7 independent joint angles respectively, u = [u1, u2, u3, u4, u5, u6, u7] T corresponding to the cable-driven rotation angles of the driving joint angles, g = [g1, g2, g3, g4, g5, g6, g7] T corresponding to the kinematic relationship between each independent joint angle and the cable-driven rotation angle.

[0102] Write the above equation in the general form similar to Equation (2):

[0103] x(k + 1) = f{x(k), u(k)} (8)

[0104] where f = [f1, f2, f3, f4, f5, f6, f7] T , and f represents the function corresponding to each independent joint angle.

[0105] A2: Linearization of the non-linear system

[0106] After establishing the state - space equation, the system is a non - linear system and needs to be linearized. The preferred embodiment of the present invention adopts the linearization method of Taylor series expansion. Taylor series expansion is a mathematical tool that approximates a non - linear function as a linear function near a certain equilibrium point by retaining the low - order terms of the non - linear function and ignoring the high - order terms. This approximation enables us to use the well - established methods of linear - system theory to analyze and design control systems, thus greatly simplifying the problem. However, this linearization method is only valid near the equilibrium point. The farther away from the equilibrium point, the greater the linearization error. When designing the control algorithm, the equilibrium point after iteration needs to be continuously updated to minimize the linearization error.

[0107] The first - order approximation form of Taylor series expansion is as follows:

[0108]

[0109] Where, and respectively represent the partial derivatives of f(x, u) with respect to x and u, calculated at the equilibrium point (x0, u0). These partial derivatives are usually called the Jacobian matrix of the system.

[0110] Substituting the above approximation into the system dynamic equation, the linearized system dynamic equation can be obtained:

[0111]

[0112] Let

[0113]

[0114] [[ID=2,8]]This linearized system dynamic equation can be written in matrix form:

[0115]

[0116] Solve the matrix partial derivative

[0117]

[0118] Since the coefficient of x in the f(x, u) function is 1 and shows a linear relationship, equation (12) can be simplified to

[0119] [[ID=,43]]

[0120] Where, D(0) = g(u0)-θ d (0), which is a constant vector.

[0121] Similarly, at time k, that is

[0122]

[0123] where D(k - 1) = g(u * (k - 1)) - θ d (k - 1) is a constant vector.

[0124] It is expressed as

[0125]

[0126] where Δu(k) = u(k) - u * (k - 1), A represents the identity matrix, B represents the linearization matrix, and D represents the constant compensation term.

[0127] In the formula, the rest and n ≠ m. By solving equations (13) and (14), the known matrices A and B can be obtained. Substituting them into the state equation (17) for further solution, it should be noted that the known quantity u of the updated matrix B each time is the optimal control strategy u * (k - 1) obtained in the previous step.

[0128] A3: Establish a prediction model

[0129] It is stipulated that at time k, the predicted value of the state variable is X k , and the predicted value of the control strategy (i.e., the input variable) is U k , and

[0130] X k = [x(k|k), x(k + 1|k), …, x(k + N|k)] T (18)

[0131] U k = [Δu(k|k), Δu(k + 1|k), …, Δu(k + N - 1|k)] T (19)

[0132] In equation (18), x(k|k) is an n×1 matrix, then X k is an (N + 1)n×1 matrix; Δu(k|k) is a p×1 matrix, then U k is an Np×1 matrix; where N is the prediction horizon, and n = p = 7.

[0133] Establish the prediction model at time k, that is

[0134]

[0135] where x(k) is the known quantity at the initial time, and equation (20) is expressed in vector form as

[0136] X k = Mx(k) + CUk +ND (21)

[0137] M = [I, A, A 2 , …, A N T (22)

[0138]

[0139] where M is an (N + 1)n × n matrix and C is an (N + 1)n × Np matrix.

[0140] A4: Optimization problem solving

[0141] Let the output y = x and the expected state value R = 0, then the error E

[0142] E = y - R = x - 0 = x (24)

[0143] Design the cost function J as

[0144]

[0145] where F and Q are the weighted matrices for the prediction end point and process points, and R is the control weighted matrix. The above equation (25) in vector form is

[0146]

[0147]

[0148] Substitute equation (21) into equation (26). In the optimization solving process, the constant term has no influence on the solution result. To simplify the operation, remove the constant term ND in equation (21) and only use the first two equations for the operation, that is

[0149]

[0150] Let Then equation (28) is simplified to

[0151] min J = x T (k)Gx(k) + 2x T (k)EU k +U k T hu k (29)

[0152] Since x(k) is the initial known condition and x T (k)Gx(k) is a known quantity and has no influence when finding the optimal solution of the cost function, so it can be simplified and only the last two equations in equation (29) are used for the operation.

[0153] ​In the MPC control algorithm, the optimization problem for the cost function J is usually a quadratic programming problem (QP). Its goal is to find a vector that minimizes or maximizes the value of a quadratic function under the constraints of some linear equalities and inequalities. The standard form of the quadratic programming problem is expressed as

[0154]

[0155] where x is the vector to be optimized, P is a symmetric positive definite quadratic term coefficient matrix, and c is a coefficient vector.

[0156] After simplifying Equation (29), it is written in the standard quadratic programming form

[0157]

[0158] The objective function of this quadratic programming consists of two parts: one part is the performance index of the system, such as the sum of the squares of the control errors, which corresponds to the optimization goal of the system; the other part is the sum of the squares of the changes in the control inputs, which corresponds to the smoothness of the controller, that is, it is desired that the control inputs change as little as possible between consecutive time instants. Once the quadratic programming problem is constructed, existing optimization solvers (such as QP solvers) can be used to solve this problem, thereby obtaining the optimal control input U at the current time k , that is, the optimal control sequence Δu(k|k), Δu(k + 1|k), …, Δu(k + N - 1|k).

[0159] A5: Iterative solution

[0160] Take the first item Δu(k|k) in the optimal control sequence as the optimal control strategy Δu at the current time * (k), and the optimal input quantity can be obtained

[0161] u * (k) = Δu * (k) + u * (k - 1) (32)

[0162] Then substitute it into Equation (7) to obtain the initial state value x(k + 1) at the next time

[0163] x(k + 1) = f{x(k), u * (k)} = x(k) + g{u * (k)}-θ d (k + 1) (33)

[0164] Update the linearization matrix B

[0165]

[0166] Repeat the process of step A5 iteratively until the robotic arm completes the trajectory movement along the desired trajectory. Summarize the processes of steps A1 to A5, and the motion control algorithm flow of the rope-driven robotic arm based on the MPC control law is as Figure 4 shown. First, initialize the system state, use the initial system state as the equilibrium point, solve the initial linearization matrix B, obtain the current system state, and set the iteration number k = 1 (the iteration number k here is equivalent to the aforementioned time k); construct a prediction model and solve the cost function to obtain the optimal control strategy; update the system state and the optimal strategy according to the optimal control strategy and the current system state; judge whether the iteration number k is less than the maximum iteration number. If so, set k = k + 1, update the linearization matrix B according to the updated optimal strategy, and return to the step of constructing the prediction model. If not, terminate the iteration, and perform motion control on the rope-driven robotic arm in combination with the updated system state and the optimal strategy.

[0167] In the third part, verify the control algorithm of the present invention.

[0168] Adopt the MPC control method to perform motion control on the spatial semi-circular trajectory. The comparison diagrams of the target trajectory and the actual trajectory of the joint angles in the arc part are as Figure 5a shown; the comparison diagrams of the target trajectory and the actual trajectory of the joint angles in the straight line part are as Figure 5b shown; the comparison diagrams of the target trajectory and the actual trajectory of the rope-driven rotation angles in the arc part are as Figure 6a shown; the comparison diagrams of the target trajectory and the actual trajectory of the rope-driven rotation angles in the straight line part are as Figure 6b shown; the comparison diagrams of the target trajectory and the actual trajectory of the changes in the XYZ coordinates of the end at each moment in the arc part are as Figure 7a shown; the comparison diagrams of the target trajectory and the actual trajectory of the changes in the XYZ coordinates of the end at each moment in the straight line part are as Figure 7b shown. It can be seen from Figures 5a to 7b that after adding the solution of the MPC control algorithm, the actual trajectory of the end and the desired target trajectory of the end basically coincide.

[0169] To sum up, the motion control method of the rope-driven robotic arm based on model predictive control proposed in the preferred embodiment of the present invention has the following advantages:

[0170] (1) Improve the control accuracy and flexibility of complex systems: Based on the model predictive control method, the present invention effectively solves the nonlinear and coupling problems of the multi-input multi-output system in the complex redundant rope-driven robotic arm; through the MPC strategy to predict and optimize the future state of the system, the present invention realizes high-precision tracking and control of the robotic arm motion trajectory, significantly improving the flexibility and control accuracy of the system in complex task scenarios.

[0171] (2) Reduce the difficulty of constructing the control model of the complex redundant cable-driven manipulator: Due to the redundant degrees of freedom of the cable-driven manipulator and the non-linear characteristics of the cables, the establishment of its control model involves the precise mapping between the operation space, joint space, and cable-driven space. Through the construction of a comprehensive control model, the present invention realizes the motion description and calculation in multiple spaces, provides the necessary data support and basic calculation framework, enabling the system to achieve precise control and complex trajectory planning.

[0172] (3) Enhance the ability to adapt to complex operation scenarios: The seven-degree-of-freedom redundant cable-driven manipulator studied in the present invention has more degrees of freedom during operation, capable of meeting the requirements of complex and multi-dimensional tasks; the combination of the compliant and highly flexible structural design and control strategy enables the manipulator to better adapt to complex and changing operation environments, such as application scenarios in industrial automation, medical rehabilitation, and logistics handling.

[0173] Another preferred embodiment of the present invention discloses a computer-readable storage medium storing a computer program, wherein the computer program is configured to be run by a processor to execute the cable-driven manipulator motion control method based on model predictive control in the above preferred embodiment.

[0174] Optionally, the above storage medium may include, but is not limited to: USB flash drive, read-only memory (ROM for short), random access memory (RAM for short), mobile hard disk, magnetic disk, or optical disc, etc., various media that can store computer programs.

[0175] The background part of the present invention may include background information about the problems or environment of the present invention, rather than the description of the prior art by others. Therefore, the content included in the background art section is not an admission by the applicant of the prior art.

[0176] The above content is a further detailed description of the present invention in combination with specific / preferred embodiments, and it cannot be determined that the specific implementation of the present invention is only limited to these descriptions. For those of ordinary skill in the technical field to which the present invention pertains, without departing from the concept of the present invention, several alternatives or modifications can be made to these described embodiments, and these alternative or modified forms should all be regarded as belonging to the protection scope of the present invention. In the description of this specification, the descriptions referring to the terms "an embodiment", "some embodiments", "preferred embodiments", "examples", "specific examples", or "some examples", etc., mean that the specific features, structures, materials, or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described can be combined in a suitable manner in any one or more embodiments or examples. In addition, without contradiction, those skilled in the art can combine and combine the different embodiments or examples described in this specification and the features of different embodiments or examples. Although the embodiments of the present invention and their advantages have been described in detail, it should be understood that various changes, substitutions, and alterations can be made herein without departing from the scope defined by the appended claims.

Claims

1. A motion control method for a cable-driven robotic arm based on model predictive control, characterized in that, Including the following steps: S1: Taking the joint angle error as the state variable and the cable drive rotation angle as the input variable, establish the nonlinear state space equation of the cable-driven manipulator according to the state variable and the input variable; S2: Perform linearization processing on the nonlinear state space equation to generate a linear approximation model, and the linear approximation model includes a linearization matrix; S3: Define a prediction interval, expand the linear approximation model into a multi-step prediction form to generate a prediction model; S4: Construct a cost function, combine the prediction model into the cost function to solve for the optimal control sequence; S5: Extract the optimal control increment at the current moment from the optimal control sequence, and update the state variable, input variable, and linearization matrix according to the optimal control increment to form a closed-loop control.

2. The motion control method of the cable-driven robotic arm according to claim 1, characterized in that The nonlinear state space equation of the cable-driven manipulator established in step S1 according to the state variable and the input variable is: x(k + 1) = x(k) + g{u(k)} - θ d (k + 1) where \(x(k)\) represents the state variable at time \(k\), \(u(k)\) represents the input variable at time \(k\), \(g\{\}\) represents the kinematic relationship between the joint angle and the cable-driven rotation angle, and \(\theta\) d (k + 1) represents the desired joint angle at time \(k+1\).

3. The motion control method of the cable-driven robotic arm according to claim 1, characterized in that The linear approximation model generated by performing Taylor series expansion linearization processing on the nonlinear state space equation in step S2 is: Where \(x(k)\) represents the state variable at time \(k\), \(\Delta u(k)=u(k) - u\) * (k - 1), \(u(k)\) represents the input variable at time \(k\), \(u\) * (k - 1) represents the optimal control input at time \(k - 1\), \(A\) represents the identity matrix, \(B\) represents the linearization matrix, and \(D\) represents the constant compensation term.

4. The motion control method of the cable-driven robotic arm according to claim 3, characterized in that, The linearization parameter B is obtained by solving the matrix partial derivative at the equilibrium point according to the first-order approximation of the Taylor series expansion, where the equilibrium point includes the state variable and input variable corresponding to the optimal control input quantity at the previous moment.

5. The motion control method of the cable-driven robotic arm according to claim 1, characterized in that, The prediction model generated in step S3 is: X k = Mx(k) + CU k + ND M = [I, A, A 2 , …, A N T ​ where X k represents the predicted value of the state variable at time k, x(k) represents the state variable at time k, U k represents the predicted value of the input variable at time k, N is the prediction horizon, D represents the constant compensation term, M is an (N + 1)n×n matrix, C is an (N + 1)n×Np matrix, N is the prediction horizon, n and p represent the degrees of freedom of the cable-driven robotic arm, A represents the identity matrix, and B represents the linearization matrix.

6. The motion control method of the cable-driven robotic arm according to claim 1, characterized in that, The cost function constructed in step S4 is: where J is the cost function, X k represents the predicted value of the state variable at time k, U k represents the predicted value of the input variable at time k, F represents the weighted matrix of the prediction end point, Q represents the weighted matrix of the prediction process points, and R represents the control weighted matrix.

7. The motion control method of the cable-driven robotic arm according to claim 6, characterized in that, Specifically, combining the prediction model into the cost function in step S4 to solve for the optimal control sequence includes: designing the optimization problem of the cost function as a quadratic programming problem including the smoothness of the control joint angle error and the control input variable, and solving the quadratic programming problem through an optimization solver to obtain the optimal control sequence at the current moment.

8. The motion control method of the cable-driven robotic arm according to claim 1, characterized in that, Forming a closed-loop control in step S5 means repeatedly executing steps S1 to S5 until the cable-driven manipulator completes the trajectory movement along the desired trajectory.

9. The motion control method of the cable-driven robotic arm according to claim 1, characterized in that, The cable-driven manipulator has a 7-degree-of-freedom redundant configuration and is composed of a two-degree-of-freedom shoulder direct drive joint, a two-degree-of-freedom elbow separated tension amplification mechanism, and a three-degree-of-freedom wrist parallel-universal joint compound transmission mechanism connected in series.

10. A computer-readable storage medium, characterized in that, A computer program is stored in the computer-readable storage medium, wherein the computer program is set to be run by a processor to execute the motion control method of the cable-driven manipulator according to any one of claims 1 to 9.

Images