Mechanical arm integrated planning control method based on Koopman linearization model

Through the combination of the deep neural network Koopman linearized model and adaptive robust controller, the problem of traditional methods being difficult to solve complex nonlinear system optimization in real time is solved, and high-performance control of the robotic arm and fast and accurate trajectory tracking are achieved.

CN118906060AActive Publication Date: 2024-11-08HAINAN RES INST OF ZHEJIANG UNIV

Patent Information

Application Number
CN202411207307.9
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-30
Publication Date
2024-11-08
Estimated Expiration
2044-08-30

AI Technical Summary

Technical Problem

Traditional control methods are difficult to solve nonlinear optimization equations of complex nonlinear systems online in real time, resulting in the inability to achieve high-performance control.

Method used

The Koopman linearization model based on deep neural network is used to linearize the nonlinear dynamic model, and combined with an adaptive robust controller, the integrated planning control of the robot arm is realized to ensure fast response and high stability under state and input constraints. state accuracy.

Benefits of technology

Real-time trajectory optimization and high-performance control of complex nonlinear systems are realized, which ensures rapid convergence and high-precision tracking of the robotic arm under state and input constraints, and improves the solution efficiency of the optimization model.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118906060A_ABST
    Figure CN118906060A_ABST
Patent Text Reader

Abstract

The invention discloses a mechanical arm integrated planning control method based on a Koopman linearization model. The method comprises the following steps: establishing a Koopman linearization model based on a deep neural network and a loss function of the Koopman linearization model, and training the Koopman linearization model so as to construct an optimization model; establishing an integrated planning control model of the mechanical arm, wherein the integrated planning control model comprises an optimization model and a self-adaptive robust controller; and inputting the actual state and the expected trajectory of the mechanical arm into the optimization model, outputting a re-planned trajectory of the expected trajectory of the mechanical arm after processing, inputting the re-planned trajectory into the self-adaptive robust controller, and outputting final control input of the mechanical arm after processing to carry out integrated planning control on the mechanical arm. According to the method, the complex nonlinear mechanical arm model is linearized, it is guaranteed that after a large tracking error occurs between the mechanical arm and the expected trajectory, rapid and high-precision convergence of the expected trajectory is achieved under the condition that constraints are met, rapid transient response performance and high steady-state tracking precision are achieved, and accurate tracking is guaranteed.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention relates to an integrated planning and control method for a robotic arm, and in particular to an integrated planning and control method for a robotic arm based on a Koopman linearization model. Background Art

[0002] In various industrial applications such as robot operation and smart car control, it is extremely important to achieve high-performance control with fast response speed and high steady-state accuracy. However, it should be noted that the ideal trajectory tracked by traditional controllers is usually generated offline, while the state and input constraints are inevitable in real robot systems. When the actual robot system has a large tracking error with the expected trajectory due to mismatched initial state or large disturbance, it is particularly important to replan the expected trajectory so that it respects the state and input constraints. Therefore, methods such as model predictive control that combine planning and control are widely used in industrial system control. However, in model predictive control, nonlinear optimization equations need to be solved based on the system's dynamic equations, which makes it impossible to obtain the numerical solution of the nonlinear optimization equations online in real time for systems with complex dynamic models, making such methods unapplicable. Therefore, it is an urgent problem to develop an online optimization solution algorithm that can be applied to complex nonlinear systems and ensure that the system has high-performance control effects. Summary of the invention

[0003] In order to solve the problems existing in the background technology, the present invention provides an integrated planning and control method for a manipulator based on a Koopman linearization model. The Koopman operator is a method that can effectively linearize a complex nonlinear dynamic model. It does this by upgrading the finite-dimensional nonlinear dynamic model to an infinite-dimensional linear system, and then finding the finite-dimensional linear system that is most similar to the infinite-dimensional linear system as the final approximate result. Therefore, the integrated planning and control method and the Koopman operator can be effectively combined.

[0004] The technical solution adopted by the present invention is:

[0005] The integrated planning and control method of a robotic arm based on a Koopman linearization model of the present invention comprises:

[0006] Step 1: Establish a Koopman linearization model based on a deep neural network and its loss function. Collect the actual state of the robot arm at consecutive moments as a training set and input it into the Koopman linearization model based on a deep neural network for training until the loss function converges to obtain the trained Koopman linearization model based on a deep neural network. Build an optimization model based on the trained Koopman linearization model based on a deep neural network.

[0007] Step 2: Establish an integrated planning and control model of the robotic arm. The integrated planning and control model includes an optimization model and an adaptive robust controller.

[0008] Step 3: Input the actual state and expected trajectory of the robot arm into the optimization model for processing, and output the re-planned trajectory of the expected trajectory of the robot arm after processing by the optimization model; input the actual state and the re-planned trajectory of the robot arm into the adaptive robust controller for processing, and output the final control input of the robot arm after processing by the adaptive robust controller, and then control the robot arm under external interference to realize integrated planning and control of the robot arm.

[0009] In step 1, the Koopman linearization model based on the deep neural network is as follows:

[0010]

[0011] Among them, ψ k , k+r and ψ k+r+1 are the state vectors of the Koopman linearized model at time k, k+r, and k+r+1 after being encoded by the deep neural network; φ() is the encoding function of the deep neural network, θ is the network weight of the deep neural network, and x k is the actual state of the robot at time k, and the actual state of the robot at time k x k Including the joint angle and angular velocity of the robot at time k; K x , K u and C are the state matrix, input matrix and decoding matrix of the Koopman linearization model respectively, K x and K u is the Koopman operator of the Koopman linearization model; u k+r is the control input of the robot at time k+r; is the predicted value of the actual state of the robot at time k+r+1.

[0012] Loss function of Koopman linearization model based on deep neural network The details are as follows:

[0013]

[0014] Among them, β1, β3 and β3 are the first, second and third weight parameters of the loss function respectively; is the prediction error of the actual state of the robot, is the prediction error of the Koopman linearization model; M is the amount of data in the training set used for deep neural network training; is the two-norm; xk+j and are the actual state of the robot at time k+j and its predicted value; ψ k+j is the state vector of the Koopman linearized model at time k+j after being encoded by the deep neural network.

[0015] The deep neural network is composed of a fully connected neural network, whose input is the current actual state of the robot arm and whose output is the system state of the Koopman linearization model; the hidden layer of the fully connected neural network is as follows:

[0016] z i =σ(W i z i-1 +b i )

[0017] σ(γ)=max(0,γ)

[0018] Among them, z i and z i-1 are the outputs of the i-th and i-1-th hidden layers respectively; σ() is the ReLU activation function, γ is the input of the ReLU activation function, and max() is the maximum value function; W i is the weight parameter of the i-th hidden layer; b i is the bias of the i-th hidden layer.

[0019] In step 1, the optimization model J is constructed based on the trained Koopman linearization model based on the deep neural network as follows:

[0020]

[0021] in, is the predicted value of the actual state of the robot at time k+r, is the set of predicted values ​​of the actual state of the robot from time 0 to time T, that is, the re-planned trajectory of the desired trajectory of the robot, T is the prediction time length; u k+r is the control input of the robot at time k+r, is the set of control inputs of the robot from time 0 to T-1; x d is the desired trajectory of the robot arm; and are weighted matrix Q and weighted norm H respectively; Ω x and Ω u is the state constraint and input constraint of the robot arm; x pq is the actual state of the qth joint of the robot, x 1q and x 2q are the actual joint angle and actual joint angular velocity of the qth joint of the robot arm, is a set of real numbers, Upq and L pq are the upper and lower limits of the actual state of the qth joint of the robot, U 1q and L 1q are the actual joint angles x of the qth joint of the robot arm 1q The upper and lower limits, U 2q and L 2q are the actual joint angular velocity x of the qth joint of the robot arm 2q The upper and lower limits of u q is the control input of the qth joint of the robot, U uq and L uq They are the upper and lower limits of the control input of the qth joint of the robot arm.

[0022] The optimization model J is solved by the QP solver.

[0023] In the step 2, the adaptive robust controller includes a parameter and disturbance estimation law and a robust control law. The actual state of the robot arm and the re-planned trajectory of the expected trajectory are input into the parameter and disturbance estimation law for processing. After the parameter and interference estimation law are processed, the uncertain parameter set of the robot arm at the next moment and the uncertainty set composed of external interference and unmodeled uncertainty are output, and input into the robust control law for processing. After the robust control law is processed, the control input of the robot arm is output.

[0024] The parameters and interference estimation law are as follows:

[0025]

[0026]

[0027] in, and are the uncertain parameter set and its estimated value in the robot arm, respectively. The uncertain parameters in the uncertain parameter set of the robot arm include the mass of the connecting rod and the position of the center of mass; is the derivative of the estimated value of the uncertain parameter set in the robot, i.e., the parameter update law; is the positive parameter update weight diagonal matrix; ε is the adaptive function; and Proj d () are the first and second mapping functions respectively; is the basis function based on the parameterized model of the manipulator; e1 and e2 are the first and second tracking errors of the optimization model and the adaptive robust controller, respectively. is the derivative of the first tracking error of the optimization model and the adaptive robust controller, k1 is the control gain of the second tracking error e2 of the optimization model and the adaptive robust controller; x and are the actual state of the robot arm and the replanned trajectory of the desired trajectory; d and are the uncertainty set and its estimated value consisting of external disturbance and unmodeled uncertainty in the robot arm, respectively. is the derivative of the estimated value of the uncertainty set consisting of the external disturbance in the robot and the unmodeled uncertainty, that is, the uncertainty set update rate; Γ Update the diagonal matrix of weights for positive uncertainty sets.

[0028] The mapping function of the parameter update law is as follows:

[0029]

[0030] in,· i is the i-th term of the mapping function of the parameter update law, is the i-th item of the estimated value of the uncertain parameter set in the robot arm; and are the uncertain parameter sets in the robot arm The lower and upper bounds of the ith item.

[0031] The mapping function of the uncertainty set update law is as follows:

[0032]

[0033] Where, · is the input of the mapping function of the deterministic set update law; is the estimated value of the uncertainty set d in the robot arm, d max and d min are the upper and lower bounds of the uncertainty set d in the robot arm, respectively.

[0034] The derivative of the estimated value according to the uncertain parameter set in the robot arm Obtain the uncertain parameter set in the robot arm at the next moment; according to the derivative of the estimated value of the uncertainty set composed of the external disturbance in the robot arm and the unmodeled uncertainty Obtain the uncertainty set consisting of external interference and unmodeled uncertainty in the robot arm at the next moment.

[0035] The robust control law is as follows:

[0036] u=u r +u s

[0037]

[0038] u s =u s1 +u s2

[0039] us1 =-k s e2

[0040]

[0041] e2u s2 ≤0

[0042]

[0043] Among them, u is the final control input of the robot arm; u s and u r are the robust control term and model compensation term of the robust control law respectively; x 1d and x 2d are the first-order expected trajectory and the second-order expected trajectory of the robot arm, respectively. is the derivative of the second-order desired trajectory of the robot, that is, the estimated value of the external disturbance of the robot; is the basis function based on the parametric model of the robot arm; is the estimated value of the uncertain parameter set in the robot arm; is the estimated value of the uncertainty set consisting of external disturbances and unmodeled uncertainties in the robot arm; u s1 and u s2 are the proportional feedback term and nonlinear robust feedback term of the robust control term of the robust control law respectively; k s is the control gain of the proportional feedback term; e2 is the second tracking error of the optimization model and the adaptive robust controller; and are parameter estimation error and uncertainty set estimation error respectively; is a positive constant that can be arbitrarily small; and are the uncertain parameter set and its estimated value in the robot arm; d and They are the uncertainty set and their estimated values ​​consisting of external disturbance and unmodeled uncertainty in the robot arm.

[0044] The electronic device of the present invention comprises: a memory and a processor coupled to each other, wherein the memory stores program data, and the processor calls the program data to execute the method as described above.

[0045] The computer-readable storage medium of the present invention stores program data thereon, and when the program data is executed by a processor, the method described above is implemented.

[0046] The method of the present invention linearizes the dynamic model of a complex nonlinear manipulator system using a Koopman model based on a deep neural network, and ensures the accuracy of the linearization result by offline training. Based on the linearization result, an optimization model is established to re-plan the desired trajectory under state and input constraints, and an adaptive robust controller is designed to accurately track the re-planned trajectory and effectively handle various uncertainties in the manipulator system. The method of the present invention can ensure that a nonlinear system with a complex dynamic model can achieve high-performance control with fast response speed and high steady-state accuracy under its own state and input constraints.

[0047] The beneficial effects of the present invention are:

[0048] 1. The integrated planning and control method proposed in the present invention can ensure that the controlled robotic arm can achieve fast and high-precision convergence to the desired trajectory while satisfying state and input constraints after a large tracking error occurs with the desired trajectory, and has fast transient response performance and high steady-state tracking accuracy.

[0049] 2. The Koopman linearization model based on deep neural network proposed in the present invention simplifies the dynamics of complex actual nonlinear robotic arm systems, thereby realizing real-time trajectory optimization and effectively improving the solution efficiency of the optimization model.

[0050] 3. The present invention ensures that the optimization model accurately tracks the optimization results of the adaptive robust controller through the parameter estimation law and uncertainty set law design of the adaptive robust controller. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 is a control flow chart of the present invention;

[0052] Figure 2 A structural diagram of a deep neural network of the present invention;

[0053] Figure 3 This is a diagram of the fitting results of the Koopman linearization model based on the deep neural network proposed in the present invention and the model fitting results based on the extended dynamic mode decomposition method, where: Figure 3 (a) is a diagram showing the fitting results of the Koopman linearization model based on a deep neural network proposed in the present invention and the model fitting results of the joint angle of the first section of the robot arm based on the extended dynamic mode decomposition method. Figure 3 (b) is a diagram showing the fitting results of the Koopman linearization model based on the deep neural network proposed in the present invention and the model fitting results of the joint angle of the second section of the robot arm based on the extended dynamic mode decomposition method. Figure 3(c) is a diagram showing the fitting results of the Koopman linearization model based on the deep neural network proposed in the present invention and the model fitting results of the joint angular velocity of the first section of the robot arm based on the extended dynamic mode decomposition method. Figure 3 (d) is a diagram showing the fitting results of the Koopman linearization model based on the deep neural network proposed in the present invention and the model fitting results of the joint angular velocity of the second section of the robot arm based on the extended dynamic mode decomposition method;

[0054] Figure 4 Schematic diagram of the angle tracking trajectory of a two-degree-of-freedom manipulator system under the model predictive control MPC method and the method of the present invention, wherein: Figure 4 (a) is a schematic diagram of the angle tracking trajectory of the first section of the two-degree-of-freedom manipulator system under the model predictive control MPC method and the method of the present invention, Figure 4 (b) is a schematic diagram of the angle tracking trajectory of the second section of the two-degree-of-freedom manipulator system under the model predictive control MPC method and the method of the present invention;

[0055] Figure 5 Schematic diagram of angular velocity tracking trajectory of a two-degree-of-freedom manipulator system under the model predictive control MPC method and the method of the present invention, wherein: Figure 5 (a) is a schematic diagram of the angular velocity tracking trajectory of the first section of the two-degree-of-freedom manipulator system under the model predictive control MPC method and the method of the present invention, Figure 5 (b) is a schematic diagram of the angular velocity tracking trajectory of the second section of the two-degree-of-freedom robotic arm system under the model predictive control MPC method and the method of the present invention. DETAILED DESCRIPTION

[0056] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0057] like Figure 1 As shown, the integrated planning and control method of the robot arm based on the Koopman linearization model of the present invention is as follows:

[0058] Step 1: Establish a Koopman linearization model based on a deep neural network and its loss function, collect the actual state of the robot arm at consecutive moments as a training set and input it into the Koopman linearization model based on a deep neural network for training until the loss function converges to obtain a trained Koopman linearization model based on a deep neural network, and construct an optimization model based on the trained Koopman linearization model based on a deep neural network. Figure 2 As shown in the figure, the Koopman linearization model based on deep neural network is as follows:

[0059]

[0060] Among them, ψ k , k+r and ψ k+r+1 are the state vectors of the Koopman linearized model at time k, k+r, and k+r+1 after being encoded by the deep neural network; φ() is the encoding function of the deep neural network, θ is the network weight of the deep neural network, and x k is the actual state of the robot at time k, and the actual state of the robot at time k x k Including the joint angle and angular velocity of the robot at time k; K x , K u and C are the state matrix, input matrix and decoding matrix of the Koopman linearization model respectively, K x and K u is the Koopman operator of the Koopman linearization model; u k+r is the control input of the robot at time k+r; is the predicted value of the actual state of the robot at time k+r+1.

[0061] Loss function of Koopman linearization model based on deep neural network The details are as follows:

[0062]

[0063] Among them, β1, β3 and β3 are the first, second and third weight parameters of the loss function respectively; is the prediction error of the actual state of the robot, is the prediction error of the Koopman linearization model; M is the amount of data in the training set used for deep neural network training; is the two-norm; x k+j and are the actual state of the robot at time k+j and its predicted value; ψ k+j is the state vector of the Koopman linearized model at time k+j after being encoded by the deep neural network.

[0064] The deep neural network is composed of a fully connected neural network, whose input is the current actual state of the robot arm and whose output is the system state of the Koopman linearization model; the hidden layer of the fully connected neural network is as follows:

[0065] z i =σ(W i z i-1 +b i )

[0066] σ(γ)=max(0,γ)

[0067] Among them, z i and z i-1 are the outputs of the i-th and i-1-th hidden layers respectively; σ() is the ReLU activation function, γ is the input of the ReLU activation function, and max() is the maximum value function; W i is the weight parameter of the i-th hidden layer; b i is the bias of the i-th hidden layer.

[0068] The optimization model J is constructed based on the trained Koopman linearization model based on deep neural network as follows:

[0069]

[0070] in, is the predicted value of the actual state of the robot at time k+r, is the set of predicted values ​​of the actual state of the robot from time 0 to time T, that is, the re-planned trajectory of the desired trajectory of the robot, T is the prediction time length; u k+r is the control input of the robot at time k+r, is the set of control inputs of the robot from time 0 to T-1; x d is the desired trajectory of the robot arm; and are weighted matrix Q and weighted norm H respectively; Ω x and Ω u is the state constraint and input constraint of the robot arm; x pq is the actual state of the qth joint of the robot, x 1q and x 2q are the actual joint angle and actual joint angular velocity of the qth joint of the robot arm, is a set of real numbers, U pq and L pq are the upper and lower limits of the actual state of the qth joint of the robot, U 1q and L 1q are the actual joint angles x of the qth joint of the robot arm 1q The upper and lower limits, U 2q and L 2q are the actual joint angular velocity x of the qth joint of the robot arm 2q The upper and lower limits of u q is the control input of the qth joint of the robot, U uq and L uq They are the upper and lower limits of the control input of the qth joint of the robot arm.

[0071] The optimization model J is solved by the QP solver.

[0072] Step 2, establish an integrated planning and control model of the manipulator, which includes an optimization model and an adaptive robust controller. The adaptive robust controller includes parameter and disturbance estimation law and robust control law, and the actual state of the manipulator and the re-planned trajectory of the desired trajectory are input into the parameter and disturbance estimation law for processing. After the parameter and disturbance estimation law is processed, the uncertain parameter set of the manipulator at the next moment and the uncertainty set composed of external disturbance and unmodeled uncertainty are output, and input into the robust control law for processing. After the robust control law is processed, the control input of the manipulator is output.

[0073] The parameters and interference estimation law are as follows:

[0074]

[0075]

[0076] in, and are the uncertain parameter set and its estimated value in the robot arm, respectively. The uncertain parameters in the uncertain parameter set of the robot arm include the mass of the connecting rod and the position of the center of mass; is the derivative of the estimated value of the uncertain parameter set in the robot, i.e., the parameter update law; is the positive parameter update weight diagonal matrix; ε is the adaptive function; and Proj d () are the first and second mapping functions respectively; is the basis function based on the parameterized model of the manipulator; e1 and e2 are the first and second tracking errors of the optimization model and the adaptive robust controller, respectively. is the derivative of the first tracking error of the optimization model and the adaptive robust controller, k1 is the control gain of the second tracking error e2 of the optimization model and the adaptive robust controller; x and are the actual state of the robot and the replanned trajectory of the desired trajectory; d and are the uncertainty set and its estimated value consisting of external disturbance and unmodeled uncertainty in the robot arm, is the derivative of the estimated value of the uncertainty set consisting of the external disturbance in the robot and the unmodeled uncertainty, that is, the uncertainty set update rate; Γ Update the diagonal matrix of weights for positive uncertainty sets.

[0077] The mapping function of the parameter update law is as follows:

[0078]

[0079] in,· i is the i-th term of the mapping function of the parameter update law, is the i-th item of the estimated value of the uncertain parameter set in the robot arm; and are the uncertain parameter sets in the robot arm The lower and upper bounds of the ith item.

[0080] The mapping function of the uncertainty set update law is as follows:

[0081]

[0082] Where, · is the input of the mapping function of the deterministic set update law; is the estimated value of the uncertainty set d in the robot arm, d max and d min are the upper and lower bounds of the uncertainty set d in the robot arm, respectively.

[0083] The derivative of the estimated value according to the uncertain parameter set in the robot arm Obtain the uncertain parameter set in the robot arm at the next moment; according to the derivative of the estimated value of the uncertainty set composed of the external disturbance in the robot arm and the unmodeled uncertainty Obtain the uncertainty set consisting of external interference and unmodeled uncertainty in the robot arm at the next moment.

[0084] The robust control law is as follows:

[0085] u=u r +u s

[0086]

[0087] u s =u s1 +u s2

[0088] u s1 =-k s e2

[0089]

[0090] e2u s2 ≤0

[0091]

[0092] Among them, u is the final control input of the robot arm; u s and u r are the robust control term and model compensation term of the robust control law respectively; x 1d and x 2d are the first-order expected trajectory and the second-order expected trajectory of the robot arm, respectively. is the derivative of the second-order desired trajectory of the robot, that is, the estimated value of the external disturbance of the robot; is the basis function based on the parametric model of the robot arm; is the estimated value of the uncertain parameter set in the robot arm; is the estimated value of the uncertainty set consisting of external disturbances and unmodeled uncertainties in the robot arm; u s1 and u s2 are the proportional feedback term and nonlinear robust feedback term of the robust control term of the robust control law respectively; k s is the control gain of the proportional feedback term; e2 is the second tracking error of the optimization model and the adaptive robust controller; and are parameter estimation error and uncertainty set estimation error respectively; is a positive constant that can be arbitrarily small; and are the uncertain parameter set and its estimated value in the robot arm; d and They are the uncertainty set and their estimated values ​​consisting of external disturbance and unmodeled uncertainty in the robot arm.

[0093] Step 3: Input the actual state and expected trajectory of the robot arm into the optimization model for processing, and output the re-planned trajectory of the expected trajectory of the robot arm after processing by the optimization model; input the actual state and the re-planned trajectory of the robot arm into the adaptive robust controller for processing, and output the final control input of the robot arm after processing by the adaptive robust controller, and then control the robot arm under external interference to realize integrated planning and control of the robot arm.

[0094] In order to verify the effectiveness and superiority of the method of the present invention, a MATLB / Simulink simulation based on a two-degree-of-freedom robotic arm was carried out on the method of the present invention, and the model fitting was compared with the extended dynamic modal decomposition method, and the control effect was compared with the model predictive control MPC (Model Predictive Control) method, wherein the prediction model of the MPC method is the Koopman linearization model based on deep neural network proposed in the present invention.

[0095] The robot model used for simulation is set to a two-degree-of-freedom robot, and the dynamic model is:

[0096]

[0097] Where q is the actual angle vector of the two-degree-of-freedom robot arm, q = [q1, q2] T ∈R 2 , is the actual angular velocity vector of the two-degree-of-freedom manipulator, is the actual angular acceleration vector of the two-degree-of-freedom manipulator, M(q) is the inertia matrix of the two-degree-of-freedom manipulator, M(q)∈R 2×2 ; is the centrifugal force and Coriolis force matrix of the two-degree-of-freedom manipulator, G(q) is the gravity matrix of the two-degree-of-freedom manipulator, G(q)∈R 2 ; U is the control input torque of the two-degree-of-freedom manipulator, U=[u1,u2] T ∈R 2 ; Δ is the sum of uncertainty and nonlinearity of the two-degree-of-freedom manipulator, d n is the nominal value of the sum of uncertainty and nonlinearity, is the nominal error of the sum of uncertainty and nonlinearity, Δ is specifically:

[0098]

[0099] Among them, ΔM(q) is the uncertain inertia matrix, is the uncertain centrifugal force and Coriolis force matrix, ΔG(q) is the uncertain gravity vector, τ f is the external interference vector.

[0100] Define the state variable x1=q∈R n ,

[0101] The dynamic matrix of the two-degree-of-freedom manipulator is specifically:

[0102]

[0103] M 11 =(m1+m2)l1 2 +m2l2 2 +2m2l1l2cos(q2)

[0104] M 12 =M 21 =m2l2 2 +m2l1l2cos(q2)

[0105] M 22 =m2l2 2

[0106]

[0107] C 22 =0

[0108] G1=(m1+m2)l1cos(q2)+m2l2cos(q1+q2)g

[0109] G2=m2l2cos(q1+q2)g

[0110] Among them, M 11 、M 12 、M 21 and M 22 are the elements of the inertia matrix M(q) of the two-degree-of-freedom manipulator; C 11 , C 12 , C 21 and C 22 The centrifugal force and Coriolis force matrices of the two-degree-of-freedom manipulator are G1 and G2 are elements in the gravity matrix G(q) of the two-degree-of-freedom manipulator; m1 and m2 are the masses of the first and second sections of the manipulator, l1 and l2 are the lengths of the first and second sections of the manipulator. In specific implementation, m1=2kg, m2=2kg, l1=0.42m, l2=0.42m; g is the acceleration due to gravity.

[0111] The state constraints and input constraints of the two-DOF manipulator system in the above simulation experiment are set as follows:

[0112] -2.8973≤q1≤2.8973,-1.7628≤q2≤1.7628

[0113]

[0114] U 1max =87,U 2max =12

[0115] like Figure 3 (a) Figure 3 (b) Figure 3 (c) and Figure 3 (d) shows the fitting results of the Koopman linearization model based on the deep neural network proposed in the present invention and the model fitting results based on the extended dynamic modal decomposition method. It can be seen from the figure that compared with the extended dynamic modal decomposition method, the Koopman linearization model fitting method based on the deep neural network proposed in the present invention has a higher degree of fit with the actual two-degree-of-freedom robot arm dynamics model, and the Koopman linearization model based on the deep neural network proposed in the present invention can achieve a longer time domain prediction effect.

[0116] like Figure 4 (a) and Figure 4 (b) is a schematic diagram of the angle tracking trajectory of a two-degree-of-freedom robotic arm system under different control methods. Figure 5 (a) and Figure 5(b) is a schematic diagram of the angular velocity tracking trajectory of a two-degree-of-freedom manipulator system under different control methods; Figure 4 and Figure 5 It can be seen that although the Koopman linearization model based on deep neural network proposed in the present invention can accurately predict the system state of the actual two-degree-of-freedom robotic arm, the inevitable prediction error between the two causes the MPC method to have a large steady-state tracking error. In addition, due to the inaccuracy of the model, the two-degree-of-freedom robotic arm even violates the angular velocity constraint under the MPC method at around 0.2-0.3s; on the contrary, since the inner-loop controller in the present invention can effectively handle uncertainty, the method proposed in the present invention can enable the two-degree-of-freedom robotic arm to achieve higher steady-state tracking accuracy and ensure that the system state constraints are always complied with.

[0117] The above contents are only the technical ideas of the present invention and cannot be used to limit the protection scope of the present invention. Any changes made on the basis of the technical solution in accordance with the technical ideas proposed by the present invention shall fall within the protection scope of the claims of the present invention.

Claims

1. A robotic arm integrated planning and control method based on Koopman linearization model, It is characterized in that include: Step 1, establish a Koopman linearization model based on a deep neural network and its loss function, collect the actual state of the robot arm at consecutive moments as a training set and input it into the Koopman linearization model based on a deep neural network for training, until the loss function converges to obtain the trained Koopman linearization model based on a deep neural network, and build an optimization model based on the trained Koopman linearization model based on a deep neural network; Step 2, establishing an integrated planning and control model of the robot arm, the integrated planning and control model includes an optimization model and an adaptive robust controller; Step 3: Input the actual state and expected trajectory of the robot arm into the optimization model for processing, and output the re-planned trajectory of the expected trajectory of the robot arm after processing by the optimization model; input the actual state and the re-planned trajectory of the expected trajectory of the robot arm into the adaptive robust controller for processing, and output the final control input of the robot arm after processing by the adaptive robust controller, and then control the robot arm under external interference to realize integrated planning and control of the robot arm.

2. According to claim 1, the integrated planning and control method of the robotic arm based on the Koopman linearization model, Features: In step 1, the Koopman linearization model based on the deep neural network is as follows: Among them, ψ k , k+r and ψ k+r+1 are the state vectors of the Koopman linearized model at time k, k+r and k+r+1 after being encoded by the deep neural network; φ( ) is the encoding function of the deep neural network, θ is the network weight of the deep neural network, and x k is the actual state of the robot at time k, and the actual state of the robot at time k x k Including the joint angle and angular velocity of the robot at time k; K x , K u and C are the state matrix, input matrix and decoding matrix of the Koopman linearization model respectively; u k+r is the control input of the robot at time k+r; is the predicted value of the actual state of the robot at time k+r+1; Loss function of Koopman linearization model based on deep neural network The details are as follows: Among them, β 1 , β 3 and β 3 are the first, second and third weight parameters of the loss function respectively; is the prediction error of the actual state of the robot, is the prediction error of the Koopman linearization model; M is the amount of data in the training set used for deep neural network training; is the two-norm; x k+j and are the actual state of the robot at time k+j and its predicted value; ψ k+j is the state vector of the Koopman linearized model at time k+j after being encoded by the deep neural network.

3. The integrated planning and control method for a robotic arm based on a Koopman linearization model according to claim 2, Features: In step 1, the optimization model J is constructed based on the trained Koopman linearization model based on the deep neural network as follows: in, is the predicted value of the actual state of the robot at time k+r, is the set of predicted values ​​of the actual state of the robot from time 0 to time T, that is, the re-planned trajectory of the desired trajectory of the robot, T is the prediction time length; u k+r is the control input of the robot at time k+r, is the set of control inputs of the robot from time 0 to T-1; x d is the desired trajectory of the robot arm; and are weighted matrix Q and weighted norm H respectively; Ω x and Ω u is the state constraint and input constraint of the robot arm; x pq is the actual state of the qth joint of the robot, x 1q and x 2q are the actual joint angle and actual joint angular velocity of the qth joint of the robot arm, is a set of real numbers, U pq and L pq are the upper and lower limits of the actual state of the qth joint of the robot, U 1q and L 1q are the actual joint angles x of the qth joint of the robot arm 1q The upper and lower limits, U 2q and L 2q are the actual joint angular velocity x of the qth joint of the robot arm 2q The upper and lower limits of u q is the control input of the qth joint of the robot, U uq and L uq They are the upper and lower limits of the control input of the qth joint of the robot arm.

4. The integrated planning and control method for a robotic arm based on a Koopman linearization model according to claim 1, Features: In the step 2, the adaptive robust controller includes a parameter and disturbance estimation law and a robust control law. The actual state of the robot arm and the re-planned trajectory of the expected trajectory are input into the parameter and disturbance estimation law for processing. After the parameter and interference estimation law are processed, the uncertain parameter set of the robot arm at the next moment and the uncertainty set composed of external interference and unmodeled uncertainty are output, and input into the robust control law for processing. After the robust control law is processed, the control input of the robot arm is output.

5. The integrated planning and control method for a robotic arm based on a Koopman linearization model according to claim 4, Features: The parameters and interference estimation law are as follows: in, and are the uncertain parameter sets and their estimated values ​​in the robot arm, respectively; is the derivative of the estimated value of the uncertain parameter set in the robot; is the positive parameter update weight diagonal matrix; ε is the adaptive function; and Proj d ( ) are the first and second mapping functions respectively; is the basis function based on the parametric model of the robotic arm; e 1 and e 2 are the first and second tracking errors of the optimization model and adaptive robust controller, respectively, is the derivative of the first tracking error of the optimization model and the adaptive robust controller, k 1 The second tracking error e is the optimization model and the adaptive robust controller 2 The control gain of x and are the actual state of the robot and the replanned trajectory of the desired trajectory; d and are the uncertainty set and its estimated value consisting of external disturbance and unmodeled uncertainty in the robot arm, is the derivative of the estimated value of the uncertainty set consisting of external disturbances and unmodeled uncertainties in the manipulator; Γ is the weight diagonal matrix for updating the positive uncertainty set; The derivative of the estimated value according to the uncertain parameter set in the robot arm Obtain the uncertain parameter set in the robot arm at the next moment; according to the derivative of the estimated value of the uncertainty set composed of the external disturbance in the robot arm and the unmodeled uncertainty Obtain the uncertainty set consisting of external interference and unmodeled uncertainty in the robot arm at the next moment.

6. The integrated planning and control method for a robotic arm based on a Koopman linearization model according to claim 4, Features: The robust control law is as follows: in=in r +in s in s =in s1 +in s2 you s1 =-k s and 2 and 2 the s2 ≤0 Among them, u is the final control input of the robot arm; u s and u r are the robust control term and model compensation term of the robust control law respectively; x 1d and x 2d are the first-order expected trajectory and the second-order expected trajectory of the robot arm, respectively. is the derivative of the second-order desired trajectory of the robot, that is, the estimated value of the external disturbance of the robot; is the basis function based on the parametric model of the robot arm; is the estimated value of the uncertain parameter set in the robot arm; is the estimated value of the uncertainty set consisting of external disturbances and unmodeled uncertainties in the robot arm; u s1 and u s2 are the proportional feedback term and nonlinear robust feedback term of the robust control term of the robust control law respectively; k s is the control gain of the proportional feedback term; e 2 The second tracking error for the optimization model and adaptive robust controller; and are parameter estimation error and uncertainty set estimation error respectively; is a positive constant; and are the uncertain parameter set and its estimated value in the robot arm; d and They are the uncertainty set and their estimated values ​​consisting of external disturbance and unmodeled uncertainty in the robot arm.

7. An electronic device, It is characterized in that include: A memory and a processor coupled to each other, wherein the memory stores program data, and the processor calls the program data to execute the method according to any one of claims 1 to 6.

8. A computer-readable storage medium having program data stored thereon, It is characterized in that When the program data is executed by a processor, the method according to any one of claims 1 to 6 is implemented.

Citation Information

Patent Citations

  • Omni-directional mobile mechanical arm data driving model prediction control method based on Koopman operator

    CN112016194A

  • Mechanical arm motion control method, system and equipment and readable storage medium

    CN114986499A

  • Mechanical arm motion planning method based on topological structure adaptive neural network

    CN115139301A

  • System and method to integrate a dynamic model for agents in a simulation environment using a deep koopman model

    US20210081808A1

Cited By

  • Dexterous hand dynamics modeling method and equipment

    CN120105927A

  • Dynamics modeling method and equipment for dexterous hands

    CN120105927B

  • Selective attention Koopman operator learning method and device guided by physical information

    CN121455061A

  • Data-driven modeling and model prediction control method for mechanical arm system

    CN122125729A

  • A Data-Driven Modeling and Model Predictive Control Method for a Robotic Arm System

    CN122125729B