Spinning creel under-actuated flexible joint mechanical arm MPC-EKF cooperative self-adaptive control method and system

Abstract

The invention belongs to the technical field of industrial robot control, and discloses a spinning creel underactuated flexible joint mechanical arm MPC-EKF cooperative self-adaptive control method and a spinning creel underactuated flexible joint mechanical arm MPC-EKF cooperative self-adaptive control system. Aiming at the problems of strong non-linear dynamics, joint flexible vibration and multi-source noise including sensor noise and environment disturbance existing in an under-actuated flexible mechanical arm system, an MPC-EKF cooperative control framework is provided, MPC predicts a future state and optimizes control input based on a linearization model, the system state is estimated in real time through the EKF, noise interference is eliminated, and the system stability is improved. And a first-order low-pass filter is combined to suppress high-frequency oscillation. According to the method, the problem of calculation complexity in the design process of the under-actuated mechanical arm controller is solved, the good control effect is achieved, the system trajectory tracking error is smaller, the convergence speed is higher, the system robustness is higher, and the method is suitable for the scene that a textile industry production robot has high requirements for dynamic precision and anti-interference performance and is low in energy consumption.

Description

Technical Field

[0001] The present invention belongs to the technical field of industrial robot control, and particularly relates to a cooperative adaptive control method and system of a textile yarn rack under-actuated flexible joint manipulator based on MPC-EKF. Background Technique

[0002] As the world's largest textile producer, China's textile industry occupies an important position in the national economy. With the development of intelligent manufacturing, the automation upgrade of textile equipment has become a key breakthrough point for industrial transformation. As a core component of modern textile production lines, the yarn rack system undertakes the key functions of yarn bobbin storage, positioning, and continuous supply. The yarn rack systems commonly used in current textile workshops are mainly divided into two categories: rail-type yarn racks and disk-type yarn racks. Rail-type yarn racks adopt a linear arrangement and have the characteristic of high space utilization; disk-type yarn racks adopt a circular layout and are more suitable for high-speed bobbin change operations. However, no matter which form, modern yarn rack systems face several prominent technical problems: First, the multi-dust environment poses strict reliability requirements on automation equipment, and the failure rate of traditional rigid manipulators increases significantly in this environment; second, there are inevitable assembly tolerances between the yarn bobbin and the yarn rack hook, requiring the actuator to have a certain degree of flexibility and adaptability.

[0003] The joint flexibility of the manipulator endows the joint part of the manipulator with the ability to adaptively adjust small angles, just like the fine rotation of the human joint. It can effectively improve the operation accuracy and can well complete processes such as grasping and handling, fiber processing, and yarn winding in textile process production. At the same time, under-actuated manipulators usually have a more lightweight and compact structure design, and the occupied space is significantly reduced compared with conventional manipulators. This highly compact structure enables it to easily adapt to various narrow working environments. For example, in a textile production workshop, the under-actuated manipulator can also have a certain degree of flexibility. Moreover, since the number of control inputs of the under-actuated system is less than the number of system degrees of freedom, this means that the energy consumption during operation is greatly reduced. Therefore, the research on the control of under-actuated flexible joint manipulators has profound engineering significance.

[0004] Model Predictive Control (MPC) is an advanced control strategy based on rolling optimization and feedback correction, and is particularly suitable for multi-variable, strongly coupled, and non-linear systems with constraint conditions. Its core idea is to generate an optimal control sequence that takes into account both the dynamic performance of the system and the satisfaction of constraints by online solving a dynamic optimization problem within a finite time domain, and apply the first control quantity to the system. This method is widely used in dealing with the control problems of under-actuated systems.

[0005] When there are unknown disturbances in the working environment of the robotic arm, the non-linear characteristics and parameter uncertainties in its dynamic model will significantly affect the control performance. If these non-linear terms or unmodeled dynamics are ignored in the controller design, it will be difficult to ensure the tracking accuracy and closed-loop stability. As a state estimation method, the Extended Kalman Filter (EKF) can provide robust state feedback for model predictive control by online linearizing the non-linear dynamic model and fusing the noise statistical characteristics, thereby indirectly compensating for the effects of system uncertainties and non-linear dynamics.

[0006] However, in the existing research, the research on model predictive control of underactuated flexible joint robotic arms based on Kalman filter is still in a blank state, and there has been no relevant report yet. Therefore, it is of great significance to design an MPC-EKF collaborative adaptive control method for underactuated flexible joint robotic arm systems. Summary of the Invention

[0007] The purpose of the present invention is to propose an MPC-EKF collaborative adaptive control method for a textile yarn rack underactuated flexible joint robotic arm, which aims at a double-link underactuated flexible joint robotic arm system with model uncertainties and realizes precise position tracking control of the underactuated flexible joint robotic arm system through model predictive control.

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

[0009] An MPC-EKF collaborative adaptive control method for a textile yarn rack underactuated flexible joint robotic arm, comprising the following steps:

[0010] Step 1. Establish the dynamic model of the underactuated flexible joint robotic arm, that is, establish a non-linear system;

[0011] Step 2. Perform explicit decoupling processing on the non-linear system based on the current state estimate value, construct the state space equation within the prediction time domain, and then perform forward Euler discretization processing to obtain the discretized model of the robotic arm system;

[0012] Step 3. Based on the discretized model of the robotic arm system, design a model predictive controller with constraints, solve the control sequence and apply the first control quantity to the controlled system, and then perform dynamic simulation on the continuous-discrete hybrid system;

[0013] Step 4. Based on the continuous-discrete hybrid system, considering the true state of the system with Gaussian noise, fuse the sensor measurement data through the extended Kalman filter algorithm to update the system state estimate in real time;

[0014] Step 5. Rollingly update the prediction horizon, and repeat Steps 2 to 4 based on the updated system state to achieve the closed-loop control of the underactuated flexible-joint manipulator.

[0015] In addition, based on the MPC-EKF cooperative adaptive control method for the underactuated flexible-joint manipulator of the textile yarn rack, the present invention also proposes a corresponding MPC-EKF cooperative adaptive control system for the underactuated flexible-joint manipulator of the textile yarn rack. The technical solution is as follows:

[0016] An MPC-EKF cooperative adaptive control system for the underactuated flexible-joint manipulator of the textile yarn rack includes the following modules:

[0017] A model construction module for establishing the dynamic model of the underactuated flexible-joint manipulator, that is, establishing a nonlinear system;

[0018] A model discretization module for performing explicit decoupling processing on the nonlinear system based on the current state estimate value, constructing the state space equation within the prediction horizon, and then performing forward Euler discretization processing to obtain the discretized model of the manipulator system;

[0019] A controller design module for designing a model predictive controller with constraints based on the discretized model of the manipulator system, solving the control sequence and applying the first control quantity to the controlled system, and then performing dynamic simulation on the continuous-discrete hybrid system;

[0020] A system state estimation module for, based on the continuous-discrete hybrid system, considering the true state of the system with Gaussian noise, fusing the sensor measurement data through the extended Kalman filter algorithm, and updating the system state estimate in real time;

[0021] And a closed-loop control module for rollingly updating the prediction horizon and achieving the closed-loop control of the underactuated flexible-joint manipulator based on the updated system state.

[0022] In addition, based on the above-mentioned MPC-EKF cooperative adaptive control method for the underactuated flexible-joint manipulator of the textile yarn rack, the present invention also proposes a computer device, which includes a memory and one or more processors;

[0023] The memory stores executable code, and when the processor executes the executable code, it is used to implement the steps of the above-mentioned MPC-EKF cooperative adaptive control method for the underactuated flexible-joint manipulator of the textile yarn rack.

[0024] In addition, based on the above MPC-EKF cooperative adaptive control method for an underactuated flexible-joint robotic arm of a textile yarn rack, the present invention also proposes a computer-readable storage medium, on which a program is stored; when the program is executed by a processor, it is used to implement the steps of the above-mentioned MPC-EKF cooperative adaptive control method for an underactuated flexible-joint robotic arm of a textile yarn rack.

[0025] The present invention has the following advantages:

[0026] As described above, the present invention relates to an MPC-EKF cooperative adaptive control method for an underactuated flexible-joint robotic arm of a textile yarn rack. First, the method of the present invention explicitly decouples the strong nonlinear characteristics of the flexible joints, solves the strong coupling and nonlinear problems existing among the underactuated state variables, and embeds the flexible coupling and inertial characteristics between the joints into the prediction model, which can avoid the control instability caused by ignoring the coupling in traditional methods. In addition, the model predictive control adopted by the method of the present invention can effectively cope with the delay effect caused by the system inertia, and solve the optimal control sequence through online quadratic programming, taking into account both the dynamic performance and the control energy consumption. Embedding the system characteristics after linearization into the prediction model can make the optimization process more conform to the actual physical constraints. In addition, the method of the present invention considers the uncertain factors in the system, and uses the extended Kalman filter to significantly improve the state estimation accuracy of the underactuated flexible-joint system under strong coupling nonlinearity and multi-source interference. The method of the present invention also solves the computational complexity problem in the controller design process of the underactuated flexible-joint robotic arm. The underactuated flexible-joint robotic arm system controlled by the method of the present invention has a smaller trajectory tracking error, a faster convergence speed, and stronger system robustness. Description of the Drawings

[0027] Figure 1 is a flowchart of the MPC-EKF cooperative adaptive control method for an underactuated flexible-joint robotic arm of a textile yarn rack in an embodiment of the present invention.

[0028] Figure 2 is a schematic diagram of an underactuated flexible-joint robotic arm system in an embodiment of the present invention.

[0029] Figure 3 is a schematic diagram of the position tracking trajectory of the system state based on the deterministic model Figure 1 .

[0030] Figure 4 is a schematic diagram of the position tracking trajectory of the system state based on the deterministic model Figure 2 .

[0031] Figure 5 is a schematic diagram of the speed tracking curve of the system state based on the deterministic model Figure 1 .

[0032] Figure 6Schematic diagram of the speed tracking curve based on the determined system state Figure 2 。

[0033] Figure 7 Schematic diagram of the tracking trajectory and observation comparison of the system state after adopting the control method of the present invention Figure 1 。

[0034] Figure 8 Schematic diagram of the tracking trajectory and observation comparison of the system state after adopting the control method of the present invention Figure 2 。

[0035] Figure 9 Schematic diagram of the tracking trajectory and noise suppression effect of the system speed state after adopting the control method of the present invention Figure 1 。

[0036] Figure 10 Schematic diagram of the tracking trajectory and noise suppression effect of the system speed state after adopting the control method of the present invention Figure 2 。

[0037] Figure 11 Schematic diagram of the tracking error of the system position after adopting the control method of the present invention Figure 1 。

[0038] Figure 12 Schematic diagram of the tracking error of the system position after adopting the control method of the present invention Figure 2 。

[0039] Figure 13 Schematic diagram of the statistical characteristics of the state estimation error after adopting the control method of the present invention Figure 1 。

[0040] Figure 14 Schematic diagram of the statistical characteristics of the state estimation error after adopting the control method of the present invention Figure 2 。

[0041] Figure 15 Schematic diagram of the statistical characteristics of the state estimation error after adopting the control method of the present invention Figure 3 。

[0042] Figure 16 Schematic diagram of the statistical characteristics of the state estimation error after adopting the control method of the present invention Figure 4 。

[0043] Figure 17 Schematic diagram of the optimized output of the control input signal after adopting the method of the present invention. Detailed implementation mode

[0044] The present invention will be further described in detail below in conjunction with the accompanying drawings and specific implementation modes:

[0045] Example 1

[0046] For the under-actuated flexible joint manipulator system with model uncertainty, in this Embodiment 1, a collaborative adaptive control method of MPC-EKF for a yarn winding frame under-actuated flexible joint manipulator is proposed. This method utilizes model predictive control technology and extended Kalman filtering technology to achieve the tracking control of the desired trajectory of the under-actuated flexible joint manipulator system.

[0047] As Figure 1 shown, a collaborative adaptive control method of MPC-EKF for a textile yarn winding frame under-actuated flexible joint manipulator specifically includes the following steps:

[0048] Step 1. Establish the dynamic model of the under-actuated flexible joint manipulator, that is, establish a nonlinear system.

[0049] The under-actuated flexible joint manipulator with 2 degrees of freedom is as Figure 2 shown, where X is the horizontal coordinate axis, Y is the vertical coordinate axis, Joint 1 is the driving joint, Joint 2 is the under-actuated flexible joint, g represents the gravitational acceleration, τ represents the control torque applied to the driving joint, q1 represents the joint angle of the driving joint, and q2 represents the joint angle of the under-actuated flexible joint.

[0050] The dynamic model of the under-actuated flexible joint manipulator with 2 degrees of freedom, that is, the nonlinear system, is expressed as:

[0051]

[0052] where, q, respectively represent the joint angle, angular velocity and angular acceleration of the double-link manipulator, q ∈ R 2 , M(q) is a symmetric positive definite inertia matrix, M(q) ∈ R 2×2 ; k c is the joint stiffness coefficient, q2 represents the joint angle of the second joint of the manipulator, is the Coriolis force centripetal matrix, G(q) is the gravity vector, τ is the control torque applied to the driving joint.

[0053] Step 2. Perform explicit decoupling processing on the nonlinear system based on the current state estimate value, construct the state space equation within the prediction time domain, and then perform forward Euler discretization processing on the current system to obtain the discretized model of the manipulator system.

[0054] Algebraically decouple Equation (1) into the form of the state space, and construct the state space equation within the prediction time domain as shown in Equation (2):

[0055]

[0056] Among them, \(x\) represents the state vector of the continuous-time system, \(u\) represents the control input of the continuous-time system, \(A(t)\) represents the continuous-time system matrix, and \(B(t)\) represents the continuous-time input matrix.

[0057]

[0058] Among them, \(x(t)\) represents the value of the state vector at time \(t\), and \(I_2\) represents the \(2\times2\) identity matrix. M 11 represents the element in the first row and first column of the inertia matrix, \(M'\) 11 represents the element in the first row and first column of the inverse matrix of the inertia matrix, \(M\) 12 represents the element in the first row and second column of the inertia matrix, \(M'\) 12 represents the element in the first row and second column of the inverse matrix of the inertia matrix, \(M\) 21 represents the element in the second row and first column of the inertia matrix, \(M'\) 21 represents the element in the second row and first column of the inverse matrix of the inertia matrix, \(M\) 22 represents the element in the second row and second column of the inertia matrix, \(M'\) 22 represents the element in the second row and second column of the inverse matrix of the inertia matrix, and \(C\) represents the Coriolis force centripetal matrix.

[0059]

[0060] Among them, \(g_1\) represents the torque component of gravity on the driving joint, and \(g_2\) represents the torque component of gravity on the underactuated flexible joint, that is, the torque component of the underactuated flexible joint.

[0061] The forward Euler method is used to discretize the continuous system shown in formula (2) to obtain a discrete-time system model:

[0062] \(x(k + 1)=A\) h \(x(k)+B\) h \(u(k)+C\) h \(c(k)\) (3)

[0063] Among them, \(k\) is the step number on the same discrete-time axis, \(x(k)\) represents the state vector of the system at the \(k\)-th discrete-time step, \(x(k + 1)\) represents the state vector of the system at the next discrete-time step, that is, the \((k + 1)\)-th discrete-time step, \(u(k)\) represents the control input applied at the \(k\)-th discrete-time step, \(c(k)\) represents the value of \(c(t)\) in the continuous-time equation of formula (2) at \(t = k\Delta t\), \(\Delta t\) represents the discrete-time step size, \(A\) h \(=(A\) k \(\Delta t+I)\), \(A\) kDenotes the value of the continuous-time system matrix A(t) at the discrete time step t = kΔt, and I denotes the identity matrix; B h = ΔtB k , B k Denotes the value of the continuous-time input matrix B(t) at the discrete time step t = kΔt; C h = IΔt.

[0064] Thus, the state expression for the next N steps of the discrete-time system model is:

[0065] x(k) = x(k)(4)

[0066] x(k + 1) = A h x(k) + B h u(k) + C h c(k)(5)

[0067] x(k + 2) = A h 2 x(k) + A h B h u(k) + B h u(k + 1) + A h C h c(k) + C h c(k)(6)

[0068]

[0069]

[0070] Where x(k + i) represents the state vector of the system at the (k + i)-th discrete time step, u(k + i) represents the control input applied at the (k + i)-th discrete time step, i = 1, 2, …, N, and N represents the prediction horizon.

[0071] According to equations (4), (5), (6), and (7), the state space expression is rewritten as:

[0072]

[0073] After arrangement, the discretized model of the robotic arm system is:

[0074] X = A b x(k) + B b U + C b c(k).

[0075] Where,

[0076] Step 3. Based on the discretized model of the robotic arm system, design a model predictive controller with constraints, solve the optimal control sequence, and apply the first control quantity to the controlled system, and then perform dynamic simulation on the continuous-discrete hybrid system.

[0077] Design a model predictive controller with constraints, solve the optimal control sequence, and apply the first control quantity to the controlled system. Let:

[0078]

[0079] where, X e represents the stack of the deviations between the actual state and the desired state within N steps of the entire prediction horizon, x e (k + i) represents the single-step error vector at the (k + i)-th step, x d (k + i) represents the desired state at the future (k + i)-th step, and X represents the stack of all predicted states from the current step k to the future (k + N)-th step; X d represents the stack of the reference trajectories from the current step k to the future (k + N)-th step.

[0080] Considering the constraints of the robotic arm system, assume that the state constraint conditions and input constraint conditions are respectively:

[0081] A c x(t) ≤ b c (10)

[0082] U min ≤ U(k) ≤ U max (11) [[ID=,36]]

[0083] where, A c represents the coefficient matrix of the state constraint, b c represents the boundary vector of the state constraint, and U(k) represents the future N-step control input sequence starting from the current time k, i.e., the control sequence.

[0084]

[0085] where, u min and u max respectively represent the minimum value and the maximum value of the control input sequence U(k).

[0086] Combined with formula (8), formula (10) is equivalent to:

[0087] A cb B b U ≤ b cb -A cb A b x(k) - A cb C b c(k) (12)

[0088] Where U is U(k).

[0089]

[0090] Convert the objective function J into the standard quadratic programming form:

[0091]

[0092] Among them, x e (N) represents the terminal error state, Q N represents the terminal state error weight matrix, x e (i) represents the error state of step i, and u(i) represents the control input of step i.

[0093]

[0094] Among them, Q represents the state error weight matrix, and R represents the control input weight matrix.

[0095] Substituting formula (8) into formula (13) and expanding it, we get:

[0096]

[0097] Expand and simplify formula (14) to obtain:

[0098]

[0099] in,

[0100]

[0101] Convert the MPC problem into a QP problem and design a constrained model predictive controller as shown in formula (16):

[0102]

[0103] The control sequence U for the next N steps is obtained by solving the quadprog function. Only the first element in the sequence (i.e., the first control variable) is used as the current control input. The current control input is substituted into the dynamic equation of the system discrete state space equation shown in formula (3). The state change is solved within one time step by numerical integration, and the system state at the next moment is finally updated to realize the dynamic simulation of the continuous-discrete hybrid system. The above-mentioned continuous-discrete hybrid system belongs to the hybrid operation mode of continuous-time dynamics and discrete-time control.

[0104] Step 4. Based on the continuous-discrete hybrid system in Step 3, considering the true state of the system with Gaussian noise, fuse the sensor measurement data through the extended Kalman filter algorithm to update the system state estimation in real time.

[0105] The state-space expression of the true state of the underactuated flexible-joint robotic arm system with Gaussian noise is:

[0106] x k = Ax k-1 + Bu k-1 + Cc k-1 + w k-1 (17)

[0107] where, x k represents the state variable of the system at the current time, i.e., at time k, and x k-1 represents the state variable of the system at the previous time, i.e., at time k-1. u k-1 represents the control input at the previous time, c k-1 represents the gravity compensation term at time k+1, and w k-1 represents the random perturbation containing Gaussian noise. ε is a sufficiently small finite perturbation value. ε is a commonly used small increment in numerical calculations, which is used to approximate partial derivatives; e n is the nth unit vector. The unit vector is a standard perturbation method for specific variable dimensions in partial derivative calculations; f j (x + εe n , u) represents the jth output value of the function after applying the positive perturbation +ε to the nth component of the state x, and f j (x - εe n , u) represents the jth output value of the function after applying the negative perturbation -ε to the nth component of the state x; B represents the value of the continuous-time input matrix B(t) at the discrete time step t = kΔt, and C = IΔt.

[0108] The sensor measurement model is:

[0109] z k = H k x k + v k (18)

[0110] where, z k represents the sensor measurement data, H k is the observation matrix, and v k represents the random error during sensor measurement.

[0111] Based on the extended Kalman filter algorithm, fuse the sensor measurement data and update the system state estimation:

[0112]

[0113] Among them, is the prior estimate of the state variable x(k) at time k; is the posterior state estimate of the state variable at time k; is the posterior estimate of the state variable at time k - 1, and K k represents the Kalman gain, is the prior estimate error covariance matrix; P k-1 is the posterior estimate error covariance matrix at time k - 1, P k is the posterior estimate error covariance matrix at time k, Q k represents the process noise covariance matrix, R k represents the measurement noise covariance matrix.

[0114] Step 5. Rollingly update the prediction horizon, and repeat Steps 2 to 4 based on the updated system state to achieve closed-loop control of the underactuated flexible-joint manipulator.

[0115] In Step 5, the specific implementation method of rollingly updating the prediction horizon is as follows:

[0116] Within the discrete time step Δt, update the system state x(k + 1) at the next moment through numerical integration, and advance the prediction horizon window by one step, truncate the oldest control quantity u(k) at the current moment, retain the subsequent control sequence u(k + 1), u(k + 2), …, u(k + N - 1), and at the same time generate a new control sequence based on the updated reference trajectory X d to keep the prediction horizon with a fixed length N.

[0117] Based on the updated system state x(k + 1), re - execute the model linearization in Step 2, the MPC optimization in Step 3, and the updated state estimation in Step 4, and achieve closed-loop control through the rolling horizon strategy to ensure the trajectory tracking stability of the underactuated flexible-joint system.

[0118] The MPC - EKF collaborative adaptive control method for the underactuated flexible-joint manipulator of the textile yarn rack described in this Embodiment 1 adopts a collaborative control strategy integrating model prediction and state estimation, which involves the trajectory tracking control of the underactuated flexible-joint manipulator in the textile scenario, and can solve the problem of insufficient accuracy of traditional methods under strong nonlinearity, joint flexible vibration, and noise interference, and is particularly suitable for the position tracking control of the underactuated flexible-joint manipulator system with model uncertainty.

[0119] The method of the present invention is a composite control method based on extended Kalman filter and model predictive control. Aiming at the problems of strong nonlinear dynamics, joint flexible vibration and multi-source noise including sensor noise and environmental disturbance existing in the underactuated flexible manipulator system, the present invention proposes an MPC-EKF collaborative control framework: MPC predicts future states based on a linearized model and optimizes control inputs, and EKF is used to estimate the system state in real time and eliminate noise interference, combined with a first-order low-pass filter to suppress high-frequency oscillations. The method of the present invention has good control effects, solves the computational complexity problem in the design process of the underactuated manipulator controller, makes the system trajectory tracking error smaller, the convergence speed faster, and the system robustness stronger, and is applicable to scenarios where textile industrial production robots have high requirements for dynamic accuracy and anti-interference ability and low energy consumption.

[0120] In addition, in order to verify the effectiveness of the method proposed in the present invention, the following specific experiments are also given:

[0121] Experiment 1

[0122] The following parameters are selected for simulation of the underactuated flexible manipulator control system:

[0123] Select the physical parameters of the manipulator: m1 = m2 = 1 kg; L1 = 1 m; L2 = 2 m; k c = 2.0 N·m; J1 = 0.0833 kg·m 2 ; J2 = 0.33 kg·m 2 .

[0124] Among them, m1 represents the mass of the driving joint link; m2 represents the mass of the underactuated flexible joint link, L1 represents the length of the driving joint link, L2 represents the length of the underactuated flexible joint link, J1 represents the moment of inertia of the driving joint link, and J2 represents the moment of inertia of the underactuated flexible joint link.

[0125] Select the reference signals: x0 = [0; 0; 0; 0], x d = [-1; -0.5; 0; 0].

[0126] Among them, x0 represents the initial state of the underactuated flexible joint manipulator system, and x d represents the desired state of the underactuated flexible joint manipulator system.

[0127] Select the controller parameters as: Q = diag([30, 30, 9, 9]); R = 0.01; Q N = Q.

[0128] Select the Kalman filter parameters:

[0129] Q k= diag([0.0001, 0.0001, 0.0005, 0.0005]); R k = diag([0.05, 0.05, 0.08, 0.08]); P k = diag([2, 2, 2, 2]).

[0130] Select the control constraint parameter: u min = -10; u max = 40; A c = [0 0 -1 0; 0 0 0 -1; 0 0 1 0; 0 0 0 1]; b c = [10; 10; 10; 10].

[0131] Select the prediction horizon: N = 34.

[0132] Figure 3 and Figure 4 is the position tracking trajectory of the system state based on the determined model, where x1 represents the actual position of the driving joint link, represents the reference position of the driving joint link, x2 represents the actual position of the underactuated flexible joint link, represents the reference position of the underactuated flexible joint. Figure 5 and Figure 6 is the speed tracking curve of the system state based on the determined model. Figure 7 and Figure 8 is the tracking trajectory and observation comparison of the system state after adopting the method of the present invention, where rod 1 is the driving joint link and rod 2 is the underactuated flexible joint link. Figure 9 and Figure 10 is the tracking trajectory and noise suppression effect of the system speed state after adopting the method of the present invention. Figure 11 and Figure 12 is the schematic diagram of the tracking error of the system position after adopting the method of the present invention, where e1 = x1 - x 1d , e1 represents the tracking error of the driving joint link, x 1d represents the reference trajectory of the driving joint link, e2 = x2 - x 2d , e2 represents the tracking error of the underactuated flexible joint link, x 2d represents the reference trajectory of the underactuated flexible joint link. Figures 13 to 16 is the statistical characteristics of the state estimation error after adopting the method of the present invention, where state 1 is the angle of the driving joint, state 2 is the angle of the underactuated flexible joint, state 3 is the angular velocity of the driving joint, and state 4 is the angular velocity of the underactuated flexible joint. Figure 17 is the optimized output of the control input signal after adopting the method of the present invention.

[0133] For an underactuated flexible-joint manipulator, the present invention can well achieve the tracking control of the target trajectory, with small tracking error, fast convergence speed, and can well handle different degrees of uncertainties in the system model, and the system has strong robustness. From the simulation results, the method of the present invention has achieved the expected goals of improving the control effect and reducing the computational complexity under the condition of model uncertainty.

[0134] Experiment 2

[0135] This Experiment 2 describes the application of the method of the present invention in a three-degree-of-freedom underactuated flexible-joint manipulator system in a textile industry grasping scenario, and further verifies the actual effectiveness of the MPC-EKF cooperative adaptive control method for the underactuated flexible-joint manipulator of a textile yarn rack under the requirements of high dynamic accuracy and anti-interference.

[0136] For the dynamic requirements of the textile production line, a three-degree-of-freedom underactuated flexible manipulator model is constructed. The end of the manipulator needs to grasp a high-speed moving textile coil while suppressing the vibration of the flexible joint. Random yarn tension disturbances are introduced into the system model to simulate multi-source disturbances in actual production.

[0137] A reference signal is generated according to the movement trajectory of the coil, and velocity and acceleration constraints are embedded in the prediction time domain to ensure smooth tracking of the manipulator. The input torque limit is dynamically adjusted according to the load, such as the change in the weight of the coil, to avoid driving overload.

[0138] The extended Kalman filter fuses multi-sensor data, including joint encoders and visual positioning. Aiming at the high-frequency vibration noise in the textile workshop, the process noise covariance is optimized in real time to improve the state estimation accuracy.

[0139] The model predictive controller online rolls to solve the optimal control sequence, giving priority to suppressing the end jitter, while taking into account the tracking error and energy consumption. The control period is synchronized with the production line beat to ensure fast response.

[0140] Implementation effect:

[0141] Tracking accuracy: The position tracking error is less than ±0.5 mm, which is 60% higher than that of the traditional PID control.

[0142] Anti-interference: Under random yarn tension disturbances, the joint vibration amplitude is reduced by 45%.

[0143] Energy consumption optimization: By constraining the dynamic torque limit, the energy consumption is reduced by 20%, meeting the energy-saving requirements of textile equipment.

[0144] Conclusion: This experiment is verified through an actual industrial scenario, indicating that the MPC-EKF cooperative adaptive control method for the underactuated flexible joint manipulator of the textile yarn rack proposed by the present invention can effectively solve the problems of flexible vibration suppression and multi-source interference compensation in high-speed and high-precision grasping tasks, providing a reliable solution for the application of underactuated manipulators in light industry automation.

[0145] Embodiment 2

[0146] This Embodiment 2 describes an MPC-EKF cooperative adaptive control system for an underactuated flexible joint manipulator of a textile yarn rack. This system and the MPC-EKF cooperative adaptive control method for the underactuated flexible joint manipulator of the textile yarn rack in Embodiment 1 are based on the same inventive concept.

[0147] Specifically, this MPC-EKF cooperative adaptive control system for an underactuated flexible joint manipulator of a textile yarn rack includes the following modules:

[0148] A model construction module for establishing the dynamic model of the underactuated flexible joint manipulator, that is, establishing a nonlinear system.

[0149] A model discretization module for performing explicit decoupling processing on the nonlinear system based on the current state estimate value, constructing the state space equation within the prediction horizon, and then performing forward Euler discretization processing to obtain the discretized model of the manipulator system.

[0150] A controller design module for designing a model predictive controller with constraints based on the discretized model of the manipulator system, solving the control sequence and applying the first control quantity to the controlled system, and further performing dynamic simulation on the continuous-discrete hybrid system.

[0151] A system state estimation module for considering the true state of the system with Gaussian noise based on the continuous-discrete hybrid system, and fusing the sensor measurement data through the extended Kalman filter algorithm to update the system state estimate in real time.

[0152] And a closed-loop control module for rolling upating the prediction horizon and implementing closed-loop control of the underactuated flexible joint manipulator based on the updated system state.

[0153] It should be noted that in the MPC-EKF cooperative adaptive control system for the underactuated flexible joint manipulator of the textile yarn rack, the implementation processes of the functions and roles of each functional module are specifically detailed in the implementation processes of the corresponding steps in the method in Embodiment 1, and will not be elaborated here.

[0154] Embodiment 3

[0155] This Embodiment 3 describes a computer device, which includes a memory and one or more processors.

[0156] Executable code is stored in a memory, and when a processor executes the executable code, it is used to implement the steps of the MPC-EKF cooperative adaptive control method for the underactuated flexible joint manipulator of the textile yarn rack in the above-mentioned Embodiment 1.

[0157] In this embodiment, the computer device is any device or apparatus with data processing capabilities, which will not be elaborated here.

[0158] Embodiment 4

[0159] This Embodiment 4 describes a computer-readable storage medium, on which a program is stored. When the program is executed by a processor, it is used to implement the steps of the MPC-EKF cooperative adaptive control method for the underactuated flexible joint manipulator of the textile yarn rack.

[0160] The computer-readable storage medium can be an internal storage unit of any device or apparatus with data processing capabilities, such as a hard disk or memory, or an external storage device of any device with data processing capabilities, such as a plug-in hard disk, a Smart Media Card (SMC), an SD card, a Flash Card, etc. equipped on the device.

[0161] Of course, the above description is only the preferred embodiments of the present invention. The present invention is not limited to listing the above embodiments. It should be noted that all equivalent substitutions and obvious deformation forms made by any person skilled in the art under the teaching of this specification fall within the substantial scope of this specification and should be protected by the present invention.

Claims

1. A collaborative adaptive control method of MPC-EKF for an underactuated flexible joint manipulator of a textile yarn rack, characterized in that, It includes the following steps: Step 1. Establish the dynamic model of the underactuated flexible joint manipulator, that is, establish a nonlinear system; Step 2. Based on the current state estimate, perform explicit decoupling processing on the nonlinear system, construct the state space equation within the prediction horizon, and then perform forward Euler discretization to obtain the discretized model of the manipulator system; Step 3. Based on the discretized model of the manipulator system, design a model predictive controller with constraints, solve the control sequence and apply the first control quantity to the controlled system, and then perform dynamic simulation on the continuous-discrete hybrid system; Step 4. Based on the continuous-discrete hybrid system, considering the true state of the system with Gaussian noise, fuse the sensor measurement data through the extended Kalman filter algorithm to update the system state estimate in real time; Step 5. Rollingly update the prediction horizon, and repeat Steps 2 to 4 based on the updated system state to achieve closed-loop control of the underactuated flexible joint manipulator.

2. The MPC-EKF collaborative adaptive control method for the underactuated flexible joint manipulator of the textile yarn rack according to claim 1, wherein Specifically, Step 1 is as follows: The dynamic model of the underactuated flexible joint manipulator with 2 degrees of freedom is expressed as: where q, are the joint angle, angular velocity, and angular acceleration of the double-link manipulator respectively, q ∈ R 2 , M(q) is a symmetric positive definite inertia matrix, M(q) ∈ R 2×2 ; k c is the joint stiffness coefficient, q2 represents the joint angle of the underactuated flexible joint of the manipulator, is the Coriolis force centripetal matrix, G(q) is the gravity vector, G(q) ∈ R 2 ; τ is the control torque applied to the driving joint.

3. The MPC-EKF cooperative adaptive control method for the underactuated flexible joint manipulator of the textile yarn rack according to claim 2, wherein Specifically, Step 2 is as follows: Algebraically decouple Equation (1) into the form of the state space, and construct the state space equation within the prediction horizon as shown in Equation (2): where, \(x\) represents the state vector of the system, \(u\) represents the control input of the system, \(A(t)\) represents the continuous-time system matrix, and \(B(t)\) represents the continuous-time input matrix; where \(x(t)\) represents the value of the state vector at time \(t\), and \(I_2\) represents the \(2\times2\) identity matrix. M′ 11 represents the element in the first row and first column of the inverse inertia matrix, \(M′\) 12 represents the element in the first row and second column of the inverse inertia matrix, \(M′\) 21 represents the element in the second row and first column of the inverse inertia matrix, \(M′\) 22 represents the element in the second row and second column of the inverse inertia matrix, and \(C\) represents the Coriolis force centripetal matrix. where, \(g_1\) represents the torque component of gravity on the driving joint, and \(g_2\) represents the torque component of gravity on the underactuated flexible joint; Use the forward Euler method to discretize the continuous system shown in Equation (2) to obtain the discrete-time system model: x(k + 1) = A h x(k) + B h u(k) + C h c(k) (3) where \(x(k)\) represents the state vector of the system at the \(k\)-th discrete time step, \(x(k + 1)\) represents the state vector of the system at the \((k + 1)\)-th discrete time step, \(u(k)\) represents the control input applied at the \(k\)-th discrete time step, \(c(k)\) represents the value of \(c(t)\) at \(t = k\Delta t\), \(\Delta t\) represents the discrete time step size, \(A\) h =(A k \Delta t+I), A k represents the value of the continuous-time system matrix \(A(t)\) at the discrete time step \(t = k\Delta t\), \(I\) represents the identity matrix; \(B\) h =\Delta tB k , V k represents the value of the continuous-time input matrix \(V(t)\) at the discrete time step \(t = k\Delta t\); \(C\) h =I\Delta t; The state expression of the discrete-time system model at the next \(N\) steps is: where, \(x(k + i)\) represents the state vector of the system at the \((k + i)\)-th discrete time step, \(u(k + i)\) represents the control input applied at the \((k + i)\)-th discrete time step, \(i = 1, 2, \cdots, N\), and \(N\) represents the prediction step size; According to Equations (4), (5), (6), and (7), the state space expression is rewritten as: The discretized model of the manipulator system is obtained as: X = A b x(k) + B b U + C b c(k); Among them, 4. The MPC-EKF collaborative adaptive control method for the underactuated flexible joint manipulator of the textile yarn rack according to claim 3, wherein Specifically, Step 3 is as follows: Let: where, X e represents the stacking of the deviations between the actual state and the desired state over the entire prediction horizon of N steps, and x e (k + i) represents the single-step error vector at the (k + i)-th step, and x d (k + i) represents the desired state at the future (k + i)-th step, where i = 1, 2, …, N; X represents the stacking of all predicted states from the current step k to the future (k + N)-th step; X d represents the stacking of the reference trajectories from the current step k to the future (k + N)-th step; Considering the constraints of the manipulator system, assume that the state constraint condition and the input constraint condition are respectively: A c x(t) ≤ b c (10) U min U(k) is less than or equal to U and greater than or equal to U max (11) Among them, A c represents the coefficient matrix of the state constraint, b c represents the boundary vector of the state constraint, and U(k) represents the future N-step control sequence starting from the current time k; where, u min and u max represent the minimum and maximum values of the control sequence U(k), respectively; Combined with Equation (8), Equation (10) is equivalent to: A cb B b U ≤ b cb -A cb A b x(k) - A cb C b c(k) (12) where, \(U\) is \(U(k)\); Convert the objective function \(J\) into the standard form of quadratic programming: where, x e (N) represents the terminal error state, Q N represents the terminal state error weight matrix, x e (i) represents the error state at the i-th step, and u(i) represents the control input at the i-th step; where, \(Q\) represents the state error weight matrix, and \(R\) represents the control input weight matrix; Substitute Equation (8) into Equation (13) and expand to get: Expand and simplify Equation (14) to get: where H = 2(B b T QB b + R0; Convert the MPC problem into a QP problem, and design a model predictive controller with constraints as shown in Equation (16): The control sequence U for the next N steps is obtained by solving with the quadprog function. Only the first element in the sequence (i.e., the first control quantity) is used as the current control input. The current control input is substituted into the discrete state-space equation of the system, i.e., the discrete-time system model shown in formula (3). The change in state is solved within one time step using numerical integration, and finally the system state at the next moment is updated to achieve the dynamic simulation of the continuous-discrete hybrid system.

5. The MPC-EKF collaborative adaptive control method for the underactuated flexible joint manipulator of the textile yarn rack according to claim 4, wherein In step 4, The state-space expression of the true state of the underactuated flexible-joint manipulator system with Gaussian noise is: x k = Ax k-1 + Bu k-1 + Cc k-1 + w k-1 (17) where, x k represents the state variable of the system at the current moment, x k-1 represents the state variable of the system at the previous moment, u k-1 represents the control input at the previous moment, c k-1 represents the gravity compensation term at time k + 1, w k-1 represents a random perturbation containing Gaussian noise, ε is a finite perturbation value, e n is the nth unit vector, f j (x + εe n , u) represents the jth output value of the function after applying a positive perturbation +ε to the nth component of the state x, f j (x - εe n , u) represents the jth output value of the function after applying a negative perturbation -ε to the nth component of the state x; B represents the value of the continuous-time input matrix B(t) at the discrete time step t = kΔt, C = IΔt; The sensor measurement model is: z k = H k x k + v k (18) Among them, z k represents the sensor measurement data, and H k is the observation matrix, while v k represents the random error during sensor measurement.

6. The MPC-EKF collaborative adaptive control method for the underactuated flexible joint manipulator of the textile yarn rack according to claim 5, wherein, In step 4, Based on the extended Kalman filter algorithm, the sensor measurement data is fused to update the system state estimate: Among them, is the prior estimate of the state variable x(k) at time k; is the posterior state estimate of the state variable at time k; is the posterior estimate of the state variable at time k-1, K k represents the Kalman gain, is the prior estimate error covariance matrix; P k-1 is the posterior estimate error covariance matrix at time k-1, P k is the posterior estimate error covariance matrix at time k, Q k represents the process noise covariance matrix, R k represents the measurement noise covariance matrix.

7. The MPC-EKF cooperative adaptive control method for the underactuated flexible joint manipulator of the textile yarn rack according to claim 6, wherein, In step 5, the specific implementation method of rolling up the prediction horizon is: Within the discrete time step Δt, the system state x(k+1) at the next moment is updated through numerical integration, and the prediction time domain window is advanced by one step. The oldest control input u(k) at the current moment is truncated, and the subsequent control sequence u(k+1), u(k+2), …, u(k+N-1) is retained. At the same time, based on the updated reference trajectory X d a new control sequence is generated to keep the prediction time domain at a fixed length N.

8. A MPC-EKF collaborative adaptive control system for an underactuated flexible joint manipulator of a textile yarn rack, characterized in that, It includes the following modules: A model construction module, which is used to establish the dynamic model of the underactuated flexible-joint manipulator, that is, to establish a nonlinear system; A model discretization module, which is used to perform explicit decoupling processing on the nonlinear system based on the current state estimate value, construct the state-space equation within the prediction horizon, and then perform forward Euler discretization processing to obtain the discretized model of the manipulator system; A controller design module, which is used to design a constrained model predictive controller based on the discretized model of the manipulator system, solve the control sequence and apply the first control quantity to the controlled system, and then perform dynamic simulation on the continuous-discrete hybrid system; A system state estimation module, which is used to consider the true state of the system with Gaussian noise based on the continuous-discrete hybrid system, and fuse the sensor measurement data through the extended Kalman filter algorithm to update the system state estimate in real time; And a closed-loop control module, which is used to roll up the prediction horizon and achieve the closed-loop control of the underactuated flexible-joint manipulator based on the updated system state.

9. A computer device, comprising a memory and one or more processors, wherein executable code is stored in the memory, characterized in that, When the processor executes the executable code, it implements the steps of the MPC-EKF cooperative adaptive control method for the underactuated flexible-joint manipulator of the textile yarn rack according to any one of claims 1 to 7.

10. A computer-readable storage medium having a program stored thereon, characterized in that, When the program is executed by the processor, it implements the steps of the MPC-EKF cooperative adaptive control method for the underactuated flexible-joint manipulator of the textile yarn rack according to any one of claims 1 to 7.

Images