An Integrated Planning and Control Method for Robotic Arms Based on the Koopman Linearization Model

Abstract

This invention discloses an integrated planning and control method for a robotic arm based on a Koopman linearization model. The method includes: establishing and training a Koopman linearization model and its loss function based on a deep neural network to construct an optimized model; establishing an integrated planning and control model for the robotic arm, including the optimized model and an adaptive robust controller; inputting the actual state and desired trajectory of the robotic arm into the optimized model, processing it, and outputting the replanned trajectory of the desired trajectory of the robotic arm, which is then input into the adaptive robust controller, processed, and outputting the final control input of the robotic arm for integrated planning and control. This invention linearizes the complex nonlinear robotic arm model, ensuring that even with large tracking errors, the robotic arm achieves fast and high-precision convergence to the desired trajectory while satisfying constraints. It exhibits fast transient response performance and high steady-state tracking accuracy, guaranteeing accurate tracking.

Description

Technical Field

[0001] This invention relates to an integrated planning and control method for robotic arms, specifically an integrated planning and control method for robotic arms based on the Koopman linearization model. Background Technology

[0002] In various industrial applications such as robotic arm operation and intelligent vehicle control, achieving high-performance control with fast response speed and high steady-state accuracy is extremely important. However, it is important to note that the ideal trajectory tracked by traditional controllers is usually generated offline, while real-world robotic arm systems inevitably have state and input constraints. When the actual robotic arm system experiences large tracking errors due to mismatched initial states or large disturbances, it becomes crucial to replan the desired trajectory to respect state and input constraints. Therefore, methods combining planning and control, such as model predictive control (MMDC), are widely used in industrial system control. However, MMDC requires solving nonlinear optimization equations based on the system's dynamic equations. This makes it impossible to obtain numerical solutions to nonlinear optimization equations online in real time for systems with complex dynamic models, thus rendering such methods unusable. Therefore, developing an online optimization algorithm applicable to complex nonlinear systems while ensuring high-performance control is an urgent problem to be solved. Summary of the Invention

[0003] To address the problems existing in the background technology, this invention provides an integrated planning and control method for robotic arms based on the Koopman linearization model. The Koopman operator is a method that can effectively linearize complex nonlinear dynamic models. It elevates the finite-dimensional nonlinear dynamic model to an infinite-dimensional linear system, and then finds the finite-dimensional linear system that best approximates this infinite-dimensional linear system as the final approximation result. Therefore, the integrated planning and control method and the Koopman operator can be effectively combined.

[0004] The technical solution adopted in this invention is:

[0005] The integrated planning and control method for robotic arms based on the Koopman linearization model of the present invention includes:

[0006] Step 1: Establish a Koopman linearization model based on a deep neural network and its loss function. Collect the actual state of the robotic arm at continuous time points 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 for 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 desired trajectory of the robotic arm into the optimization model for processing. After processing, the optimization model outputs the replanned trajectory of the desired trajectory of the robotic arm. Input the actual state and the replanned trajectory of the desired trajectory of the robotic arm into the adaptive robust controller for processing. After processing, the adaptive robust controller outputs the final control input of the robotic arm, thereby controlling the robotic arm under external disturbances and realizing the integrated planning and control of the robotic arm.

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

[0010]

[0011] Where, ψ k ψ k+r and ψ k+r+1 φ() represents the state vectors of the Koopman linearized model encoded by the deep neural network at times k, k+r, and k+r+1, respectively; φ() is the encoding function of the deep neural network, θ is the network weight of the deep neural network, and x is the weight of the network. k Let x be the actual state of the robotic arm at time k. k This includes the joint angles and angular velocities of the robotic arm at time k; K x K u C and K represent the state matrix, input matrix, and decoder matrix of the Koopman linearized model, respectively. x and K u The Koopman operator for the Koopman linearization model; u k+r This is the control input for the robotic arm at time k+r; This is the predicted value of the actual state of the robotic arm at time k+r+1.

[0012] Loss function of the Koopman linearization model based on deep neural networks Specifically as follows:

[0013]

[0014] Wherein, β1, β3, and β3 are the first, second, and third weight parameters of the loss function, respectively; The prediction error for the actual state of the robotic arm. denoted as the prediction error of the Koopman linearized model; M represents the amount of data in the training set used for training the deep neural network. It is a 2-norm; xk+j and These represent the actual state of the robotic arm at time k+j and its predicted value, respectively; ψ k+j This is the state vector of the Koopman linearized model encoded by a deep neural network at time k+j.

[0015] The deep neural network is composed of fully connected neural networks. Its input is the current actual state of the robotic arm, and its output is the system state of the Koopman linearized model. The hidden layers of the fully connected neural network are as follows:

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

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

[0018] Among them, z i and z i-1 σ() represents 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 b represents the weight parameters of the i-th hidden layer. i This 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 using a deep neural network as follows:

[0020]

[0021] in, Let be the predicted value of the actual state of the robotic arm at time k+r. This is the set of predicted values ​​of the actual state of the robotic arm from time 0 to T, i.e., the replanned trajectory of the robotic arm's desired trajectory, where T is the prediction time length; u k+r This is the control input for the robotic arm at time k+r. x is the set of control inputs to the robotic arm from time 0 to T-1; d This represents the desired trajectory of the robotic arm; and These are the weighted matrix Q and the weighted norm H, respectively; Ω x and Ω u State constraints and input constraints for the robotic arm; x pq Let x be the actual state of the q-th joint of the robotic arm. 1q and x 2q These are the actual joint angle and actual joint angular velocity of the q-th joint of the robotic arm, respectively. U is the set of real numbers.pq and L pq U represents the upper and lower limits of the actual state of the q-th joint of the robotic arm. 1q and L 1q The actual joint angles x of the q-th joint of the robotic arm are respectively 1q The upper and lower limits of U 2q and L 2q The actual joint angular velocity x of the q-th joint of the robotic arm are respectively 2q The upper and lower limits; u q U is the control input for the q-th joint of the robotic arm. uq and L uq These are the upper and lower limits of the control input for the q-th joint of the robotic arm, respectively.

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

[0023] In step 2, the adaptive robust controller includes a parameter and disturbance estimation law and a robust control law. The actual state of the robotic arm and the replanned trajectory of the desired trajectory are input into the parameter and disturbance estimation law for processing. After processing, the parameter and disturbance estimation law outputs the set of uncertain parameters of the robotic arm at the next moment, as well as the set of uncertainties composed of external disturbances and unmodeled uncertainties, and inputs them into the robust control law for processing. After processing, the robust control law outputs the control input of the robotic arm.

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

[0025]

[0026]

[0027] in, and These are the set of uncertain parameters in the robotic arm and their estimated values. The uncertain parameters in the set of uncertain parameters of the robotic arm include the mass of the link and the position of the center of mass. It is the derivative of the estimated value of the uncertain parameter set in the robotic arm, i.e., the parameter update law; The positive parameters update the weight diagonal matrix; ε is the adaptive function; and Proj d () are the first and second mapping functions, respectively; e1 and e2 are the basis functions based on the parametric model of the robotic arm; e1 and e2 are the first and second tracking errors of the optimized model and the adaptive robust controller, respectively. The derivative of the first tracking error of the optimization model and the adaptive robust controller is given by k1, where k1 is the control gain of the second tracking error e2 of the optimization model and the adaptive robust controller; x and These represent the actual state of the robotic arm and the replanned trajectory of the desired trajectory, respectively; d and These represent the uncertainty set and its estimated value, consisting of external disturbances and unmodeled uncertainties in the robotic arm. It is the derivative of the estimated value of the uncertainty set consisting of external disturbances and unmodeled uncertainties in the robotic arm, i.e., the uncertainty set update rate; Γ Update the weight diagonal matrix for positive uncertainty sets.

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

[0029]

[0030] in,· i Let i be the i-th term of the mapping function for the parameter update law. Let i be the i-th term of the estimated value of the set of uncertain parameters in the robotic arm; and These are the sets of uncertain parameters in the robotic arm. The upper and lower boundaries of the i-th term.

[0031] The mapping function for the update law of uncertain sets is as follows:

[0032]

[0033] Where · is the input to the mapping function of the deterministic set update law; Let d be an estimate of the uncertainty set d in the robotic arm. max and d min Let be the upper and lower boundaries of the uncertainty set d in the robotic arm, respectively.

[0034] The derivative of the estimated values ​​of the uncertain parameter set in the robotic arm Obtain the set of uncertain parameters in the robotic arm at the next moment; based on the derivatives of the estimates of the uncertainty set constituted by external disturbances and unmodeled uncertainties in the robotic arm. The uncertainty set consists of external disturbances and unmodeled uncertainties in the robotic 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] Where u is the final control input of the robotic arm; u s and u r These are the robust control term and model compensation term of the robust control law, respectively; x 1d and x 2d These are the first-order and second-order expected trajectories of the robotic arm, respectively. This is the derivative of the second-order desired trajectory of the robotic arm, which is the estimate of the external disturbances to the robotic arm. These are the basis functions based on the parametric model of the robotic arm; These are estimates of the set of uncertain parameters in the robotic arm; This is an estimate of the uncertainty set consisting of external disturbances and unmodeled uncertainties in the robotic arm; u s1 and u s2 These are the proportional feedback term and the nonlinear robust feedback term of the robust control law, respectively; k s e2 is the control gain of the proportional feedback term; e2 is the second tracking error of the optimized model and the adaptive robust controller. and These are the parameter estimation error and the uncertainty set estimation error, respectively. It is a positive number that can be arbitrarily small; and These are the set of uncertain parameters in the robotic arm and their estimated values; d and These represent the set of uncertainties and their estimates, consisting of external disturbances and unmodeled uncertainties in the robotic arm.

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

[0045] The present invention provides a computer-readable storage medium having program data stored thereon, which, when executed by a processor, implements the method described above.

[0046] This invention utilizes a Koopman model based on a deep neural network to linearize the dynamic model of a complex nonlinear robotic arm system, and ensures the accuracy of the linearization results through offline training. Based on the linearization results, an optimization model is established to replan the desired trajectory under state and input constraints, and an adaptive robust controller is designed to accurately track the replanned trajectory and effectively handle various uncertainties in the robotic arm system. This invention can guarantee high-performance control with fast response speed and high steady-state accuracy for nonlinear systems with complex dynamic models under their own state and input constraints.

[0047] The beneficial effects of this invention are:

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

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

[0050] 3. This invention ensures that the optimization model accurately tracks the optimization results of the adaptive robust controller by designing the parameter estimation law and uncertainty set law of the adaptive robust controller. Attached Figure Description

[0051] Figure 1 This is a control flowchart of the present invention;

[0052] Figure 2 This is a diagram of the deep neural network structure of the present invention;

[0053] Figure 3 The figures show the fitting results of the Koopman linearization model based on deep neural networks and the model fitting results based on the extended dynamic mode decomposition method proposed in this invention. Figure 3 Figure (a) shows the fitting results of the Koopman linearization model based on deep neural networks proposed in this invention and the model fitting results of the joint angles of the first section of the robotic arm based on the extended dynamic mode decomposition method. Figure 3 Figure (b) shows the fitting results of the Koopman linearization model based on deep neural networks proposed in this invention and the model fitting results of the joint angles of the second section of the robotic arm based on the extended dynamic mode decomposition method. Figure 3(c) shows the fitting results of the Koopman linearization model based on deep neural networks proposed in this invention and the model fitting results of the joint angular velocities of the first section of the robotic arm based on the extended dynamic mode decomposition method. Figure 3 (d) is a figure showing the fitting results of the Koopman linearization model based on deep neural network proposed in this invention and the model fitting results of the joint angular velocity of the second section of the robotic arm based on the extended dynamic mode decomposition method.

[0054] Figure 4 This is a schematic diagram of the angle tracking trajectory of a two-degree-of-freedom robotic arm system under the Model Predictive Control (MPC) method and the method of this invention. Figure 4 (a) is a schematic diagram of the angle tracking trajectory of the first section of the two-degree-of-freedom robotic arm system under the Model Predictive Control (MPC) method and the method of this 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 This is a schematic diagram of the angular velocity tracking trajectory of a two-degree-of-freedom robotic arm system under the Model Predictive Control (MPC) method and the method of this invention. Figure 5 (a) is a schematic diagram of the angular velocity tracking trajectory of the first section of the two-degree-of-freedom robotic arm system under the Model Predictive Control (MPC) method and the method of this invention. Figure 5 (b) is a schematic diagram of the angular velocity 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. Detailed Implementation

[0056] The present invention will be 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 for robotic arms 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 states of the robotic arm at continuous time points as a training set and input them into the Koopman linearization model based on the deep neural network for training until the loss function converges to obtain the trained Koopman linearization model based on the deep neural network. Then, construct an optimization model based on the trained Koopman linearization model based on the deep neural network. For example... Figure 2 As shown, the Koopman linearization model based on deep neural networks is as follows:

[0059]

[0060] Where, ψ k ψ k+r and ψ k+r+1 φ() represents the state vectors of the Koopman linearized model encoded by the deep neural network at times k, k+r, and k+r+1, respectively; φ() is the encoding function of the deep neural network, θ is the network weight of the deep neural network, and x is the weight of the network. k Let x be the actual state of the robotic arm at time k. k This includes the joint angles and angular velocities of the robotic arm at time k; K x K u C and K represent the state matrix, input matrix, and decoder matrix of the Koopman linearized model, respectively. x and K u The Koopman operator for the Koopman linearization model; u k+r This is the control input for the robotic arm at time k+r; This is the predicted value of the actual state of the robotic arm at time k+r+1.

[0061] Loss function of the Koopman linearization model based on deep neural networks Specifically as follows:

[0062]

[0063] Wherein, β1, β3, and β3 are the first, second, and third weight parameters of the loss function, respectively; The prediction error for the actual state of the robotic arm. denoted as the prediction error of the Koopman linearized model; M represents the amount of data in the training set used for training the deep neural network. It is a 2-norm; x k+j and These represent the actual state of the robotic arm at time k+j and its predicted value, respectively; ψ k+j This is the state vector of the Koopman linearized model encoded by a deep neural network at time k+j.

[0064] The deep neural network is composed of fully connected neural networks. Its input is the current actual state of the robotic arm, and its output is the system state of the Koopman linearized model. The hidden layers of the fully connected neural network are as follows:

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

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

[0067] Among them, z i and z i-1 σ() represents 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 b represents the weight parameters of the i-th hidden layer. i This is the bias of the i-th hidden layer.

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

[0069]

[0070] in, Let be the predicted value of the actual state of the robotic arm at time k+r. This is the set of predicted values ​​of the actual state of the robotic arm from time 0 to T, i.e., the replanned trajectory of the robotic arm's desired trajectory, where T is the prediction time length; u k+r This is the control input for the robotic arm at time k+r. x is the set of control inputs to the robotic arm from time 0 to T-1; d This represents the desired trajectory of the robotic arm; and These are the weighted matrix Q and the weighted norm H, respectively; Ω x and Ω u State constraints and input constraints for the robotic arm; x pq Let x be the actual state of the q-th joint of the robotic arm. 1q and x 2q These are the actual joint angle and actual joint angular velocity of the q-th joint of the robotic arm, respectively. U is the set of real numbers. pq and L pq U represents the upper and lower limits of the actual state of the q-th joint of the robotic arm. 1q and L 1q The actual joint angles x of the q-th joint of the robotic arm are respectively 1q The upper and lower limits of U 2q and L 2q The actual joint angular velocity x of the q-th joint of the robotic arm are respectively 2q The upper and lower limits; u q U is the control input for the q-th joint of the robotic arm. uq and L uq These are the upper and lower limits of the control input for the q-th joint of the robotic arm, respectively.

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

[0072] Step 2: Establish an integrated planning and control model for the robotic arm. This model includes an optimization model and an adaptive robust controller. The adaptive robust controller comprises parameter and disturbance estimation laws and a robust control law. The actual state of the robotic arm and the replanned trajectory of the desired trajectory are input into the parameter and disturbance estimation laws for processing. After processing, the parameter and disturbance estimation laws output the set of uncertain parameters of the robotic arm at the next time step, as well as the set of uncertainties consisting of external disturbances and unmodeled uncertainties. These are then input into the robust control law for processing. After processing, the robust control law outputs the control input for the robotic arm.

[0073] The parameters and disturbance estimation laws are as follows:

[0074]

[0075]

[0076] in, and These are the set of uncertain parameters in the robotic arm and their estimated values. The uncertain parameters in the set of uncertain parameters of the robotic arm include the mass of the link and the position of the center of mass. It is the derivative of the estimated value of the uncertain parameter set in the robotic arm, i.e., the parameter update law; The positive parameters update the weight diagonal matrix; ε is the adaptive function; and Proj d () are the first and second mapping functions, respectively; e1 and e2 are the basis functions based on the parametric model of the robotic arm; e1 and e2 are the first and second tracking errors of the optimized model and the adaptive robust controller, respectively. The derivative of the first tracking error of the optimization model and the adaptive robust controller is given by k1, where k1 is the control gain of the second tracking error e2 of the optimization model and the adaptive robust controller; x and These represent the actual state of the robotic arm and the replanned trajectory of the desired trajectory, respectively; d and These represent the uncertainty set and its estimated value, consisting of external disturbances and unmodeled uncertainties in the robotic arm. It is the derivative of the estimated value of the uncertainty set consisting of external disturbances and unmodeled uncertainties in the robotic arm, i.e., the uncertainty set update rate; Γ Update the weight diagonal matrix for positive uncertainty sets.

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

[0078]

[0079] in,· i Let i be the i-th term of the mapping function for the parameter update law. Let i be the i-th term of the estimated value of the set of uncertain parameters in the robotic arm; and These are the sets of uncertain parameters in the robotic arm. The upper and lower boundaries of the i-th term.

[0080] The mapping function for the update law of uncertain sets is as follows:

[0081]

[0082] Where · is the input to the mapping function of the deterministic set update law; Let d be an estimate of the uncertainty set d in the robotic arm. max and d min Let be the upper and lower boundaries of the uncertainty set d in the robotic arm, respectively.

[0083] The derivative of the estimated values ​​of the uncertain parameter set in the robotic arm Obtain the set of uncertain parameters in the robotic arm at the next moment; based on the derivatives of the estimates of the uncertainty set constituted by external disturbances and unmodeled uncertainties in the robotic arm. The uncertainty set consists of external disturbances and unmodeled uncertainties in the robotic 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] Where u is the final control input of the robotic arm; u s and u r These are the robust control term and model compensation term of the robust control law, respectively; x 1d and x 2d These are the first-order and second-order expected trajectories of the robotic arm, respectively. This is the derivative of the second-order desired trajectory of the robotic arm, which is the estimate of the external disturbances to the robotic arm. These are the basis functions based on the parametric model of the robotic arm; These are estimates of the set of uncertain parameters in the robotic arm; This is an estimate of the uncertainty set consisting of external disturbances and unmodeled uncertainties in the robotic arm; u s1 and u s2 These are the proportional feedback term and the nonlinear robust feedback term of the robust control law, respectively; k s e2 is the control gain of the proportional feedback term; e2 is the second tracking error of the optimized model and the adaptive robust controller. and These are the parameter estimation error and the uncertainty set estimation error, respectively. It is a positive number that can be arbitrarily small; and These are the set of uncertain parameters in the robotic arm and their estimated values; d and These represent the set of uncertainties and their estimates, consisting of external disturbances and unmodeled uncertainties in the robotic arm.

[0093] Step 3: Input the actual state and desired trajectory of the robotic arm into the optimization model for processing. After processing, the optimization model outputs the replanned trajectory of the desired trajectory of the robotic arm. Input the actual state and the replanned trajectory of the desired trajectory of the robotic arm into the adaptive robust controller for processing. After processing, the adaptive robust controller outputs the final control input of the robotic arm, thereby controlling the robotic arm under external disturbances and realizing the integrated planning and control of the robotic arm.

[0094] To verify the effectiveness and superiority of the method of the present invention, a MATLAB / Simulink simulation based on a two-degree-of-freedom manipulator was performed. The method was compared with the extended dynamic mode decomposition method in terms of model fitting and with the model predictive control (MPC) method in terms of control performance. The prediction model of the MPC method is the Koopman linearized model based on deep neural networks proposed in this invention.

[0095] The robotic arm model used for simulation is set as a two-degree-of-freedom robotic arm, and the dynamic model is as follows:

[0096]

[0097] Where q is the actual angle vector of the two-degree-of-freedom robotic arm, q = [q1, q2] T ∈R 2 , This is the actual angular velocity vector of the two-degree-of-freedom robotic arm. This is the actual angular acceleration vector of the two-degree-of-freedom robotic arm. M(q) is the inertia matrix of the two-degree-of-freedom robotic arm, M(q)∈R 2×2 ; The matrices of the centrifugal force and Coriolis force of a two-degree-of-freedom robotic arm. G(q) is the gravity matrix of the two-degree-of-freedom robotic arm, G(q)∈R 2 U is the control input torque of the two-degree-of-freedom robotic arm, U = [u1, u2] T ∈R 2 Δ represents the sum of uncertainties and nonlinearities in a two-degree-of-freedom robotic arm, and d n The nominal value of the sum of uncertainty and nonlinearity. The nominal error, Δ, is the sum of uncertainty and nonlinearity.

[0098]

[0099] Where ΔM(q) is the uncertain inertia matrix, Let τ be the matrix of uncertain centrifugal force and Coriolis force, ΔG(q) be the uncertain gravity vector, and τ be the matrix of uncertain centrifugal force and Coriolis force. f This is the external interference vector.

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

[0101] The specific dynamics matrix of the two-degree-of-freedom robotic arm is as follows:

[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 These are elements in the inertia matrix M(q) of the two-degree-of-freedom robotic arm; C 11 C 12 C 21 and C 22 The centrifugal force and Coriolis force matrices of a two-degree-of-freedom robotic arm are respectively. The elements in the matrix are: G1 and G2 are the elements in the gravity matrix G(q) of the two-degree-of-freedom robotic arm; m1 and m2 are the masses of the first and second robotic arms, respectively, and l1 and l2 are the lengths of the first and second robotic arms, respectively. In the specific implementation, m1 = 2kg, m2 = 2kg, l1 = 0.42m, l2 = 0.42m; g is the gravitational acceleration.

[0111] The state constraints and input constraints of the two-degree-of-freedom robotic arm 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 of (a), Figure 3 (b) Figure 3 (c) and Figure 3 As shown in (d), the fitting results of the Koopman linearization model based on deep neural network proposed in this invention and the model fitting results based on extended dynamic mode decomposition method are shown in the figure. It can be seen from the figure that, compared with the extended dynamic mode decomposition method, the Koopman linearization model based on deep neural network proposed in this invention has a higher degree of fit with the actual two-degree-of-freedom manipulator dynamics model. Furthermore, the Koopman linearization model based on deep neural network proposed in this invention can achieve prediction results in a longer time domain.

[0116] like Figure 4 (a) and Figure 4 As shown in (b), this 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 5As shown in (b), this is a schematic diagram of the angular velocity tracking trajectory of a two-degree-of-freedom robotic arm system under different control methods; Figure 4 and Figure 5 It can be seen that although the Koopman linearization model based on deep neural networks proposed in this invention can predict the system state of the actual two-degree-of-freedom manipulator relatively accurately, the unavoidable prediction error of both leads to a large steady-state tracking error in the MPC method. In addition, due to the inaccuracy of the model, the two-degree-of-freedom manipulator under the MPC method even violates the angular velocity constraint at around 0.2-0.3s. In contrast, since the inner loop controller in this invention can effectively handle uncertainty, the method proposed in this invention can enable the two-degree-of-freedom manipulator to achieve higher steady-state tracking accuracy and ensure that the system state constraints are always observed.

[0117] The above content is merely a technical concept of the present invention and should not be construed as limiting the scope of protection of the present invention. Any modifications made to the technical solution based on the technical concept proposed in this invention shall fall within the scope of protection of the claims of this invention.

Claims

1. A robotic arm integrated planning and control method based on the Koopman linearization model, 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 robotic arm at continuous time points 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. Step 2: Establish an integrated planning and control model for the robotic arm. The integrated planning and control model includes an optimization model and an adaptive robust controller. Step 3: Input the actual state and desired trajectory of the robotic arm into the optimization model for processing. After processing, the optimization model outputs the replanned trajectory of the desired trajectory of the robotic arm. Input the actual state and the replanned trajectory of the desired trajectory of the robotic arm into the adaptive robust controller for processing. After processing, the adaptive robust controller outputs the final control input of the robotic arm, thereby controlling the robotic arm under external disturbances and realizing the integrated planning and control of the robotic arm. In step 1, the Koopman linearization model based on deep neural networks is as follows: Where, ψ k ψ k+r and ψ k+r+1 Let be the state vectors of the Koopman linearized model encoded by the deep neural network at times k, k+r, and k+r+1, respectively; φ() is the encoding function of the deep neural network, θ is the network weight of the deep neural network, and x is the weight of the network. k Let x be the actual state of the robotic arm at time k. k This includes the joint angles and angular velocities of the robotic arm at time k; K x K u C and C represent the state matrix, input matrix, and decoder matrix of the Koopman linearized model, respectively; u k+r This is the control input for the robotic arm at time k+r; This is the predicted value of the actual state of the robotic arm at time k+r+1; Loss function of the Koopman linearization model based on deep neural networks Specifically as follows: Wherein, β1, β3, and β3 are the first, second, and third weight parameters of the loss function, respectively; The prediction error for the actual state of the robotic arm. denoted as the prediction error of the Koopman linearized model; M represents the amount of data in the training set used for training the deep neural network. It is a 2-norm; x k+j and These represent the actual state of the robotic arm at time k+j and its predicted value, respectively; ψ k+j This is the state vector of the Koopman linearized model encoded by a deep neural network at time k+j.

2. The integrated planning and control method for a robotic arm based on the Koopman linearization model according to claim 1, characterized in that: In step 1, the optimization model J is constructed based on the trained Koopman linearization model using a deep neural network as follows: in, Let be the predicted value of the actual state of the robotic arm at time k+r. This is the set of predicted values ​​of the actual state of the robotic arm from time 0 to T, i.e., the replanned trajectory of the robotic arm's desired trajectory, where T is the prediction time length; u k+r This is the control input for the robotic arm at time k+r. x is the set of control inputs to the robotic arm from time 0 to T-1; d This represents the desired trajectory of the robotic arm; and These are the weighted matrix Q and the weighted norm H, respectively; Ω x and Ω u State constraints and input constraints for the robotic arm; x pq Let x be the actual state of the q-th joint of the robotic arm. 1q and x 2q These are the actual joint angle and actual joint angular velocity of the q-th joint of the robotic arm, respectively. U is the set of real numbers. pq and L pq U represents the upper and lower limits of the actual state of the q-th joint of the robotic arm. 1q and L 1q The actual joint angles x of the q-th joint of the robotic arm are respectively 1q The upper and lower limits of U 2q and L 2q The actual joint angular velocity x of the q-th joint of the robotic arm are respectively 2q The upper and lower limits; u q U is the control input for the q-th joint of the robotic arm. uq and L uq These are the upper and lower limits of the control input for the q-th joint of the robotic arm, respectively.

3. The integrated planning and control method for a robotic arm based on the Koopman linearization model according to claim 1, characterized in that: In step 2, the adaptive robust controller includes a parameter and disturbance estimation law and a robust control law. The actual state of the robotic arm and the replanned trajectory of the desired trajectory are input into the parameter and disturbance estimation law for processing. After processing, the parameter and disturbance estimation law outputs the set of uncertain parameters of the robotic arm at the next moment, as well as the set of uncertainties composed of external disturbances and unmodeled uncertainties, and inputs them into the robust control law for processing. After processing, the robust control law outputs the control input of the robotic arm.

4. The integrated planning and control method for a robotic arm based on the Koopman linearization model according to claim 3, characterized in that: The parameters and disturbance estimation law are as follows: in, and These are the set of uncertain parameters in the robotic arm and their estimated values; It is the derivative of the estimated value of the uncertain parameter set in the robotic arm; The positive parameters update the weight diagonal matrix; ε is the adaptive function; Proj θ ( ) and Proj d ( ) represent the first and second mapping functions, respectively; e1 and e2 are the basis functions based on the parametric model of the robotic arm; e1 and e2 are the first and second tracking errors of the optimized model and the adaptive robust controller, respectively. The derivative of the first tracking error of the optimization model and the adaptive robust controller is given by k1, where k1 is the control gain of the second tracking error e2 of the optimization model and the adaptive robust controller; x and These represent the actual state of the robotic arm and the replanned trajectory of the desired trajectory, respectively; d and These represent the uncertainty set and its estimated value, consisting of external disturbances and unmodeled uncertainties in the robotic arm. Γ is the derivative of the estimated value of the uncertainty set consisting of external disturbances and unmodeled uncertainties in the robotic arm; Γ is the positive uncertainty set update weight diagonal matrix; The derivative of the estimated values ​​of the uncertain parameter set in the robotic arm Obtain the set of uncertain parameters in the robotic arm at the next moment; based on the derivatives of the estimates of the uncertainty set constituted by external disturbances and unmodeled uncertainties in the robotic arm. The uncertainty set consists of external disturbances and unmodeled uncertainties in the robotic arm at the next moment.

5. The integrated planning and control method for a robotic arm based on the Koopman linearization model according to claim 3, characterized in that: The robust control law is as follows: in=in r +in s in s =in s1 +in s2 u s1 =-k s e2 e2u s2 ≤0 Where u is the final control input of the robotic arm; u s and u r These are the robust control term and model compensation term of the robust control law, respectively; x 1d and x 2d These are the first-order and second-order expected trajectories of the robotic arm, respectively. This is the derivative of the second-order desired trajectory of the robotic arm, which is the estimate of the external disturbances to the robotic arm. These are the basis functions based on the parametric model of the robotic arm; These are estimates of the set of uncertain parameters in the robotic arm; This is an estimate of the uncertainty set consisting of external disturbances and unmodeled uncertainties in the robotic arm; u s1 and u s2 These are the proportional feedback term and the nonlinear robust feedback term of the robust control law, respectively; k s e2 is the control gain of the proportional feedback term; e2 is the second tracking error of the optimized model and the adaptive robust controller. and These are the parameter estimation error and the uncertainty set estimation error, respectively. It is a positive number; and These are the set of uncertain parameters in the robotic arm and their estimated values; d and These represent the set of uncertainties and their estimates, consisting of external disturbances and unmodeled uncertainties in the robotic arm.

6. An electronic device, characterized in that, include: A memory and a processor are coupled to each other, wherein the memory stores program data, and the processor invokes the program data to perform the method as described in any one of claims 1-5.

7. A computer-readable storage medium storing program data thereon, characterized in that, When the program data is executed by the processor, the method as described in any one of claims 1-5 is implemented.

Images