Mechanical arm integrated planning control method based on nonlinear optimization

Through the integrated planning control method of robotic arms based on nonlinear optimization, a dynamic model under state constraints and input constraints is established, and a dual-loop control model is constructed, which solves the compliance constraints and high accuracy of robotic arms under uncertain dynamics and external interference. Tracking problems, achieving fast response and high-precision control effects.

CN119057777AInactive Publication Date: 2024-12-03ZHEJIANG UNIV

Patent Information

Application Number
CN202411207309.8
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-30
Publication Date
2024-12-03
Estimated Expiration
Not applicable · inactive patent

AI Technical Summary

Technical Problem

Existing robotic arm control algorithms are difficult to comply with state and input constraints while achieving fast response and high-precision tracking in the presence of uncertain dynamics and external interference.

Method used

Using the integrated planning control method of robotic arm based on nonlinear optimization, the nonlinear dynamic state space model under state constraints and input constraints is established to build a dual-ring control model for planning control, using the outer ring planner and the inner ring controller. Perform optimization generation and interference estimation to achieve optimized control of the robotic arm.

Benefits of technology

Under the premise of complying with state and input constraints, the fast transient response and high steady-state tracking accuracy of the robot arm are achieved, improving the robustness and dynamic tracking capabilities of the system.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119057777A_ABST
    Figure CN119057777A_ABST
Patent Text Reader

Abstract

The invention discloses a mechanical arm integrated planning control method based on nonlinear optimization. The method comprises the steps that a planning and control integrated double-loop control model comprising an outer loop planner and an inner loop controller is established based on a nonlinear dynamic state space model of the mechanical arm; an actual track and an expected track of a mechanical arm are input into an optimization generator of an outer ring planner, the actual track is input into a direct model generator, optimization control input and an optimization track are output after processing, a switcher inputs the selected optimization control input and optimization track into an inner ring controller after switching selection, and the optimal control input and the optimization track are output. And after processing, outputting final control input to carry out integrated planning control on the mechanical arm. According to the method, after a large tracking error occurs between the controlled mechanical arm and the expected trajectory, the rapid and high-precision convergence effect on the expected trajectory can be achieved under the condition that constraints are met, rapid transient response performance and high steady-state tracking precision are achieved, the robustness of the system is effectively improved, and the calculation complexity is reduced.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention relates to a robot arm integrated planning and control method, and in particular to a robot arm integrated planning and control method based on nonlinear optimization. Background Art

[0002] As the requirements for the performance of the robot arm in various applications such as medical treatment, industrial manufacturing, and marine exploration become higher and higher, the robot arm controller is required to meet multiple design goals such as fast response speed, high tracking accuracy, and good closed-loop stability. However, the existence of the highly coupled nonlinear dynamics of the robot arm itself, various uncertain dynamics, and inevitable physical constraints increase the difficulty of controller design. Traditional control laws are difficult to meet multi-objective control. The uncertain dynamics of the robot arm include uncertain inertia, uncertain friction torque, uncertain gravity torque, and unknown external interference. If the controller is designed by ignoring the influence of these uncertain parts on the robot arm, it may cause the system's motion performance to deteriorate or even cause the system to be unable to operate stably. The various physical constraints of the robot arm can be mainly divided into two categories: state constraints and input constraints. The state constraints are caused by the joint workspace and operation speed restrictions, including joint angle constraints and joint angular velocity constraints. The input constraints refer to the output of the actuator within a certain range. If the system does not comply with these constraints, problems such as reduced control accuracy and system instability will also occur. Among the existing control algorithms, the control strategies used to solve parameter uncertainty and external disturbances include: adaptive control, Smith predictive control, disturbance observer control, neural network control; the methods used to deal with system state and input constraints include: barrier Leyap function, nonlinear mapping, predictive performance control, optimization control, etc. However, so far, there is no practical method that can make the robot arm not only comply with state and input constraints, but also have fast transient response performance and high steady-state tracking accuracy in the presence of uncertain dynamics and external disturbances. Summary of the invention

[0003] In order to solve the problems existing in the background technology, the present invention provides a robot arm integrated planning and control method based on nonlinear optimization.

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

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

[0006] Step 1: Establish a nonlinear dynamic state space model of the robot under state constraints and input constraints.

[0007] Step 2, based on the nonlinear dynamic state space model of the robot arm, a planning and control integrated dual-loop control model is established. The planning and control integrated dual-loop control model includes an outer loop planner and an inner loop controller. The outer loop planner includes an optimization generator, a direct model generator and a switcher.

[0008] Step 3, input the actual trajectory and expected trajectory of the robot arm into the optimization generator of the outer loop planner for processing, and input the actual trajectory of the robot arm into the direct model generator of the outer loop planner for processing. After processing, the optimization generator and the direct model generator both output the optimized control input and optimized trajectory of the robot arm; the switch of the outer loop planner makes a judgment based on the actual trajectory of the robot arm and switches between the optimization generator and the direct model generator. The outer loop planner inputs the optimized control input and optimized trajectory of the robot arm output by the optimization generator or the direct model generator selected by the switch into the inner loop controller for processing. After processing, the inner loop controller outputs the final control input of the robot arm, thereby performing integrated planning and control of the robot arm.

[0009] In step 1, the nonlinear dynamic state space model of the robot arm is as follows:

[0010]

[0011] Among them, x 1 and are the first state variable and its derivative, i.e., the actual angle vector and actual angular velocity vector of the robot arm, x 2 and are the second state variable and its derivative, namely the actual angular velocity vector and actual angular acceleration vector of the manipulator; M(x 1 ) is based on the first state variable x 1 The inertia matrix of the manipulator; U is the control input torque of the manipulator, U = [u 1 ,u 2 ,…,u n ] T ∈R n ,u 1 ,u 2 ,…,u n are the control input torques of the 1st, 2nd, ..., nth joints of the robot arm respectively; C(x 1 ,x 2 ) is based on the first state variable x 1 and the second state variable x 2 The centrifugal force and Coriolis force matrix of the manipulator; G(x 1 ) is based on the first state variable x 1 is the gravity matrix of the robotic arm; Δ is the sum of the uncertainty and nonlinearity of the robotic arm.

[0012] The nonlinear dynamic model of the robotic arm system is as follows:

[0013]

[0014] Where q is the actual angle vector of the robot arm, q = [q 1 ,q 2 ,...,q n ] T ∈R n ,q 1 ,q 2 ,...,q n are the actual angles of the 1st, 2nd, ..., nth joints of the robot arm, is the actual angular velocity vector of the robot arm, are the actual angular velocities of the 1st, 2nd, ..., nth joints of the robot arm, is the actual angular acceleration vector of the robot arm, are the actual angular accelerations of the 1st, 2nd, ..., nth joints of the robot arm; M(q) is the inertia matrix of the robot arm, M(q)∈R n×n ; is the centrifugal force and Coriolis force matrix of the robot, G(q) is the gravity matrix of the robot arm, G(q)∈R n ;d n is the nominal value of the sum of uncertainty and nonlinearity, is the nominal error of the sum of uncertainty and nonlinearity; Δ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.

[0015] Define the state variable x 1 =q∈R n , Thus, the state space form of the nonlinear dynamic model is obtained.

[0016] The state constraints of the nonlinear dynamic state space model of the robot are as follows:

[0017] x 1 ∈[x 1min ,x 1max ]

[0018] x 2 ∈[x 2min ,x 2max ]

[0019] Among them, x1max and x 1min are the first state variables x 1 The upper and lower boundary vectors of x 2max and x 2min are the second state variables x 2 The upper and lower boundary vectors of .

[0020] The input constraints of the nonlinear dynamic state space model of the manipulator are as follows:

[0021] U∈[-U max ,U max ]

[0022] Among them, U max It is the maximum value vector of the control input torque U of the robot arm.

[0023] In step 2, the optimization generator of the outer loop planner is as follows:

[0024]

[0025] K′=[K 1 ,-I]

[0026]

[0027] Among them, U r is the optimization result of the equivalent model compensation of the control input torque U of the robot arm; 1r and are the expected trajectory x of the joint angles of the robot arm d The optimization results and their derivatives, x 2r and are the expected trajectories of the joint angular velocities of the robot arm The optimization results and their derivatives of T f is the prediction time length, t 0 is the initial time of the sampling period; M(x 1r ) is the expected trajectory x based on the joint angle of the robot arm d The optimization result x 1r The inertia matrix of 1r ,x 2r ) is the expected trajectory x based on the joint angle of the robot arm d The optimization result x 1r and the desired trajectory of the joint angular velocity The optimization result x 2r The centrifugal and Coriolis force matrices; G(x 1r ) is the expected trajectory x based on the joint angle of the robot arm d The optimization result x 1r The gravity matrix of x1 r(t 0 ) and x 2 r(t 0 ) are t 0 The expected trajectory of the joint angle of the robot arm at time x d and the desired trajectory of the joint angular velocity The optimization result of x 1 (t 0 ) and x 2 (t 0 ) are t 0 The actual trajectory of the joint angle and joint angular velocity of the robot arm at the moment x 1 r(t 0 +Tf) and x 2 r(t 0 +Tf) are t 0 +T f The expected trajectory xd of the joint angle and the expected trajectory of the joint angular velocity of the robot at the moment The optimization result of x d (t 0 +T f )and t 0 +T f The expected trajectory of the joint angle and angular velocity of the robot arm at the moment; U max is the maximum value vector of the control input torque U of the manipulator; η is the model compensation error scaling factor matrix, ε is the feedback term scaling factor matrix, δ d is the scaling factor for interference compensation; (x 1min ) i and (x 1max ) i are the expected trajectory x of the joint angles of the robot arm d The optimization result x 1r The minimum state constraint matrix x 1min and the maximum state constraint matrix x 1max The i-th row element in (Δx 1min ) i and (Δx 1max ) i are the expected trajectory x of the joint angles of the robot arm d The optimization result x 1r The minimum state constraint matrix x 1min The scaling factor matrix Δx 1min and the maximum state constraint matrix x 1max The scaling factor matrix Δx 1max The i-th row element in (x 2min ) i and (x 2max )i are the expected trajectory x of the joint angular velocity of the robot arm d The optimization result x 2r The minimum state constraint matrix x 2min and the maximum state constraint matrix x 2max The i-th row element of (Δx 2min ) i and (x 2max ) i are the expected trajectories of the joint angular velocities of the robot arm The optimization result x 2r The minimum state constraint matrix x 2min The scaling factor matrix Δx 2min and the maximum state constraint matrix x 2max The scaling factor matrix Δx 2max The i-th row element of ; c is a positive constant parameter; max() is the maximum value function; (I)i is the i-th row element of the identity matrix I; P′ is the first weighted matrix norm of the scaling factor matrix of the state constraint matrix, diag[] is a diagonal matrix, P 1 is the first tracking error z between the inner loop controller and the outer loop planner 1 The weighted matrix norm of , M is the inertia matrix; (K') i is the i-th row element of the second weighted matrix norm K' of the scaling factor matrix of the state constraint matrix, K 1 is the sliding surface control gain; (A) i is the i-th row element of the positive first constant diagonal matrix A related to the model compensation error; η i and ε i are the i-th element in the model compensation error scaling factor matrix η and the feedback term scaling factor matrix ε respectively; (K s ) i is the gain matrix K of the robust feedback term s The i-th row element of .

[0028] The optimization generator is solved by the SQP solver, and the optimized control input of the robot arm output by the optimization generator is the optimization result U of the control input torque U of the robot arm r , the optimized trajectory of the robot output by the optimization generator includes the expected trajectory x of the joint angles of the robot d The optimization result x 1r and the desired trajectory of the joint angular velocity The optimization result x 2r .

[0029] The positive first constant diagonal matrix A associated with the model compensation error satisfies the following constraints:

[0030] |(H) i|≤|(Az) i |

[0031]

[0032] Among them, (H) i The model compensation error matrix is The i-th row element of is; z is the first tracking error z between the inner loop controller and the outer loop planner 1 and the second tracking error z 2 The total tracking error vector is composed of z = [z 1 ,z 2 ] T ; (Az) i is the i-th row element of the multiplication matrix Az of the first positive constant diagonal matrix A and the total tracking error vector z; M(x 1 ) is the actual angle vector x based on the robot arm 1 The inertia matrix of 1 ,x 2 ) is the actual angle vector x based on the robot arm 1 and the actual angular velocity vector x of the robot arm 2 The centrifugal and Coriolis force matrices; G(x 1 ) is the actual angle vector x based on the robot arm 1 The gravity matrix.

[0033] In step 2, the direct model generator of the outer loop planner is as follows:

[0034] x 1r =x d

[0035]

[0036] Among them, U d is the optimization result of the direct model equivalent compensation of the control input torque U of the robot arm; 1r and x 2r are the expected trajectory x of the joint angles of the robot arm d and the desired trajectory of the joint angular velocity The optimization result of is the expected trajectory of the joint angular acceleration of the robot; M(x d ) is the expected trajectory x based on the joint angle of the robot arm d The inertia matrix of is the expected trajectory x based on the joint angles of the robot d and the desired trajectory of the joint angular velocity The centrifugal and Coriolis force matrices; G(x d ) is the expected trajectory x based on the joint angle of the robot armd The gravity matrix.

[0037] The optimal control input of the manipulator output by the direct model generator is the optimal result U of the control input torque U of the manipulator r , the optimized trajectory of the robot output by the direct model generator includes the expected trajectory x of the joint angles of the robot d The optimization result x 1r and the desired trajectory of the joint angular velocity The optimization result x 2r .

[0038] In step 2, the switch of the outer loop planner is as follows:

[0039]

[0040] e=xx d

[0041]

[0042] Among them, V s () is the triggering Lyapunov function; e is the actual joint angle x and the expected joint angle x of the robot arm. d The tracking error; ξ is the actual joint angular velocity of the robot and the desired joint angular velocity The tracking error of P 1 is the first tracking error z between the inner loop controller and the outer loop planner 1 The weighted matrix norm of ; M is the inertia matrix; K 1 is the sliding surface control gain.

[0043] When the Lyapunov function V is triggered s (e,ξ)>c s When , the optimization generator of the outer loop planner is switched; when the Lyapunov function V is triggered s (e,ξ)≤c s , then switch to the direct model generator of the outer loop planner, c s is the constant threshold of the switch.

[0044] In the step 2, the inner loop controller is an adaptive robust controller based on disturbance estimation. The inner loop controller includes a disturbance estimator and a saturation controller. The optimized trajectory of the manipulator is input into the disturbance estimator. After processing, the disturbance estimator outputs the observed value of the nominal value of the sum of uncertainty and nonlinearity and the robust feedback term to the saturation controller. At the same time, the optimized control input of the manipulator is input into the saturation controller. After processing, the saturation controller outputs the final control input of the manipulator. The inner loop controller is specifically as follows:

[0045] a) Interference Estimator:

[0046]

[0047] z 1 =x 1 -x 1r

[0048]

[0049] in, and are the nominal values ​​of the sum of uncertainty and nonlinearity of the robot arm, respectively. n The observed value and its derivative; Proj() is the projection mapping function; γ d is a positive constant diagonal matrix; z 1 and z 2 are the first and second tracking errors of the inner loop controller and the outer loop observer, respectively; is the first tracking error z between the inner loop controller and the outer loop observer 1 The derivative of K 1 is the sliding surface control gain; x 1 and x 2 are the actual angle vector and actual angular velocity vector of the robot arm, respectively, 1r and x 2r are the expected trajectory x of the joint angles of the robot arm d and the desired trajectory of the joint angular velocity The optimization results.

[0050] The disturbance estimator outputs the nominal value d of the sum of uncertainty and nonlinearity of the robot arm at the current moment n The derivative of the observed value Then, the nominal value d of the sum of uncertainty and nonlinearity of the robot arm at the next moment is obtained. n Observed value to the saturation controller.

[0051] The projection mapping function Proj() is as follows:

[0052]

[0053] in,· i is the i-th term of the projection mapping function; (d) i is the nominal value of the sum of uncertainty and nonlinearity of the robot arm d n Observed value The i-th term of di is the boundary value of the i-th term of the uncertainty and nonlinear sum of the robot arm.

[0054] b) Saturation controller:

[0055] U=σ(z)U s +(1-σ(z))U o

[0056]

[0057] U s =[u s1 ,u s1 ,...,u si ,...,u sn ] T

[0058] u si =-sign((z 2 ) i ) imax ,i=1,2,...,n

[0059] U o =U x +dK s z

[0060]

[0061] Among them, U is the final control input of the robot arm, U s and U o are the saturation control law and the adaptive robust control law, respectively, u s1 ,u s2 ,...,u si ,...,u sn are the 1st, 2nd, …, i, …, nth elements in the saturation control law, U imax is the maximum value of the i-th element in the saturation control law, sign() is the sign function; σ() and V() are the judgment function and Lyapunov function respectively; c is a positive constant parameter; P 1 is the first tracking error z between the inner loop controller and the outer loop planner 1 The weighted matrix norm of ; M is the inertia matrix; U x The optimal control input U of the manipulator output by the optimization generator of the outer loop planner r Or the optimized control input U of the robot arm output by the direct model generator d ; K s is the robust feedback term K s The gain matrix of z; Q 2 is a positive second constant-valued diagonal matrix.

[0062] 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.

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

[0064] The method of the present invention is based on an inner-loop adaptive robust controller to estimate and compensate for external interference in real time, so as to ensure that the manipulator system has accurate dynamic tracking capability and strong robustness. According to the tracking error size of the actual expected trajectory, a switch is used to select and trigger the optimization generator and the direct model generator of the outer-loop planner, so as to effectively reduce the computational complexity. At the same time, the optimization generator re-optimizes the expected trajectory under large tracking error to obtain the optimized trajectory that complies with the constraints and transmits it to the inner-loop system, so as to ensure that the system complies with the constraints. The method of the present invention can ensure that the manipulator system can comply with the state and input constraints and has fast transient response performance and high steady-state tracking accuracy when there is dynamic uncertainty and external interference.

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

[0066] 1. The integrated planning control framework designed in the present invention can ensure that the controlled robotic arm system can achieve fast and high-precision convergence to the desired trajectory under the premise of complying with 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.

[0067] 2. The present invention utilizes the disturbance estimator of the inner loop controller to estimate and compensate for external disturbances in real time, thereby effectively improving the robustness of the system.

[0068] 3. The outer loop planner of the present invention uses a switch to trigger the optimization generator or the direct model generator, which effectively reduces the computational complexity. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0070] Figure 2 Schematic diagram of the angle tracking trajectory of the robotic arm system under the method of the present invention and the control method based on the reference regulator, wherein: Figure 2 (a) is a schematic diagram of the angle tracking trajectory of the first section of the manipulator of the manipulator system under the control method of the present invention and the reference regulator, Figure 2 (b) is a schematic diagram of the angle tracking trajectory of the second section of the manipulator of the manipulator system under the method of the present invention and the control method based on the reference regulator;

[0071] Figure 3 Schematic diagram of the angle tracking error trajectory of the manipulator system under the method of the present invention and the control method based on the reference regulator, wherein: Figure 3 (a) is a schematic diagram of the angle tracking error trajectory of the first section of the manipulator system under the control method of the present invention and the reference regulator, Figure 3 (b) is a schematic diagram of the angle tracking error trajectory of the second section of the manipulator of the manipulator system under the control method of the present invention and the reference regulator;

[0072] Figure 4 Schematic diagram of the angular velocity tracking trajectory of the manipulator system under the method of the present invention and the control method based on the reference regulator, wherein: Figure 4 (a) is a schematic diagram of the angular velocity tracking trajectory of the first section of the manipulator system under the control method of the present invention and the reference regulator, Figure 4 (b) is a schematic diagram of the angular velocity tracking trajectory of the second section of the manipulator of the manipulator system under the control method of the present invention and the reference regulator;

[0073] Figure 5 Schematic diagram of control signals of a robotic arm system under the method of the present invention and the control method based on a reference regulator, wherein: Figure 5 (a) is a schematic diagram of control signals of the first section of the manipulator of the manipulator system under the control method of the present invention and the reference regulator, Figure 5 (b) is a schematic diagram of control signals of the second section of the robotic arm of the robotic arm system under the method of the present invention and the control method based on the reference regulator. DETAILED DESCRIPTION

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

[0075] like Figure 1 As shown, the integrated planning and control method of the robot arm based on nonlinear optimization of the present invention is as follows:

[0076] Step 1: Establish a nonlinear dynamic state space model of the manipulator under state constraints and input constraints, as follows:

[0077]

[0078] Among them, x 1 and are the first state variable and its derivative, i.e., the actual angle vector and actual angular velocity vector of the robot arm, x 2 and are the second state variable and its derivative, namely the actual angular velocity vector and actual angular acceleration vector of the manipulator; M(x1 ) is based on the first state variable x 1 The inertia matrix of the manipulator; U is the control input torque of the manipulator, U = [u 1 ,u 2 ,…,u n ] T ∈R n ,u 1 ,u 2 ,…,u n are the control input torques of the 1st, 2nd, ..., nth joints of the robot arm respectively; C(x 1 ,x 2 ) is based on the first state variable x 1 and the second state variable x 2 The centrifugal force and Coriolis force matrix of the manipulator; G(x 1 ) is based on the first state variable x 1 is the gravity matrix of the robotic arm; Δ is the sum of the uncertainty and nonlinearity of the robotic arm.

[0079] The nonlinear dynamic model of the robotic arm system is as follows:

[0080]

[0081] Where q is the actual angle vector of the robot arm, q = [q 1 ,q 2 ,...,q n ] T ∈R n ,q 1 ,q 2 ,...,q n are the actual angles of the 1st, 2nd, ..., nth joints of the robot arm, is the actual angular velocity vector of the robot arm, are the actual angular velocities of the 1st, 2nd, ..., nth joints of the robot arm, is the actual angular acceleration vector of the robot arm, are the actual angular accelerations of the 1st, 2nd, ..., nth joints of the robot arm; M(q) is the inertia matrix of the robot arm, M(q)∈R n×n ; is the centrifugal force and Coriolis force matrix of the robot, G(q) is the gravity matrix of the robot arm, G(q)∈R n ;d n is the nominal value of the sum of uncertainty and nonlinearity, is the nominal error of the sum of uncertainty and nonlinearity; Δ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.

[0082] Define the state variable x 1 =q∈R n , Thus, the state space form of the nonlinear dynamic model is obtained.

[0083] The state constraints of the nonlinear dynamic state space model of the robot are as follows:

[0084] x 1 ∈[x 1min ,x 1max ]

[0085] x 2 ∈[x 2min ,x 2max ]

[0086] Among them, x 1max and x 1min are the first state variables x 1 The upper and lower boundary vectors of x 2max and x 2min are the second state variables x 2 The upper and lower boundary vectors of .

[0087] The input constraints of the nonlinear dynamic state space model of the manipulator are as follows:

[0088] U∈[-U max ,U max ]

[0089] Among them, U max It is the maximum value vector of the control input torque U of the robot arm.

[0090] Step 2, based on the nonlinear dynamic state space model of the robot arm, a planning and control integrated dual-loop control model is established. The planning and control integrated dual-loop control model includes an outer loop planner and an inner loop controller. The outer loop planner includes an optimization generator, a direct model generator and a switcher.

[0091] The optimization generator of the outer loop planner is as follows:

[0092]

[0093] K′=[K 1 ,-I]

[0094]

[0095] Among them, U ris the optimization result of the equivalent model compensation of the control input torque U of the robot arm; 1r and are the expected trajectory x of the joint angles of the robot arm d The optimization results and their derivatives, x 2r and are the expected trajectories of the joint angular velocities of the robot arm The optimization results and their derivatives of T f is the prediction time length, t 0 is the initial time of the sampling period; M(x 1r ) is the expected trajectory x based on the joint angle of the robot arm d The optimization result x 1r The inertia matrix of 1r ,x 2r ) is the expected trajectory x based on the joint angle of the robot arm d The optimization result x 1r and the desired trajectory of the joint angular velocity The optimization result x 2r The centrifugal and Coriolis force matrices; G(x 1r ) is the expected trajectory x based on the joint angle of the robot arm d The optimization result x 1r The gravity matrix of x 1 r(t 0 ) and x 2 r(t 0 ) are t 0 The expected trajectory of the joint angle of the robot arm at time x d and the desired trajectory of the joint angular velocity The optimization result of x 1 (t 0 ) and x 2 (t 0 ) are t 0 The actual trajectory of the joint angle and joint angular velocity of the robot arm at the moment x 1r (t 0 +T f ) and x 2r (t 0 +T f ) are t 0 +T f The expected trajectory of the joint angle of the robot arm at time x d and the desired trajectory of the joint angular velocity The optimization result of x d (t 0 +T f )and t 0 +T fThe expected trajectory of the joint angle and angular velocity of the robot arm at the moment; U max is the maximum value vector of the control input torque U of the manipulator; η is the model compensation error scaling factor matrix, ε is the feedback term scaling factor matrix, δ d is the scaling factor for interference compensation; (x 1min ) i and (x 1max ) i are the expected trajectory x of the joint angles of the robot arm d The optimization result x 1r The minimum state constraint matrix x 1min and the maximum state constraint matrix x 1max The i-th row element in (Δx 1min ) i and (Δx 1max ) i are the expected trajectory x of the joint angles of the robot arm d The optimization result x 1r The minimum state constraint matrix x 1min The scaling factor matrix Δx 1min and the maximum state constraint matrix x 1max The scaling factor matrix Δx 1max The i-th row element in (x 2min ) i and (x 2max ) i are the expected trajectory x of the joint angular velocity of the robot arm d The optimization result x 2r The minimum state constraint matrix x 2min and the maximum state constraint matrix x 2max The i-th row element of (Δx 2min ) i and (x 2max ) i are the expected trajectories of the joint angular velocities of the robot arm The optimization result x 2r The minimum state constraint matrix x 2min The scaling factor matrix Δx 2min and the maximum state constraint matrix x 2max The scaling factor matrix Δx 2max The i-th row element of ; c is a positive constant parameter; max() is the maximum value function; (I) i is the i-th row element of the identity matrix I; P′ is the first weighted matrix norm of the scaling factor matrix of the state constraint matrix, diag[] is a diagonal matrix, P 1 is the first tracking error z between the inner loop controller and the outer loop planner 1The weighted matrix norm of , M is the inertia matrix; (K') i is the i-th row element of the second weighted matrix norm K' of the scaling factor matrix of the state constraint matrix, K 1 is the sliding surface control gain; (A) i is the i-th row element of the positive first constant diagonal matrix A related to the model compensation error; η i and ε i are the i-th element in the model compensation error scaling factor matrix η and the feedback term scaling factor matrix ε respectively; (K s ) i is the gain matrix K of the robust feedback term s The i-th row element of .

[0096] The optimization generator is solved by the SQP solver, and the optimized control input of the robot arm output by the optimization generator is the optimization result U of the control input torque U of the robot arm r , the optimized trajectory of the robot output by the optimization generator includes the expected trajectory x of the joint angles of the robot d The optimization result x 1r and the desired trajectory of the joint angular velocity The optimization result x 2r .

[0097] The positive first constant diagonal matrix A associated with the model compensation error satisfies the following constraints:

[0098] |(H) i |≤|(Az) i |

[0099]

[0100] Among them, (H) i The model compensation error matrix is The i-th row element of is; z is the first tracking error z between the inner loop controller and the outer loop planner 1 and the second tracking error z 2 The total tracking error vector is composed of z = [z 1 ,z 2 ] T ; (Az) i is the i-th row element of the multiplication matrix Az of the first positive constant diagonal matrix A and the total tracking error vector z; M(x 1 ) is the actual angle vector x based on the robot arm 1 The inertia matrix of 1 ,x 2 ) is the actual angle vector x based on the robot arm 1 and the actual angular velocity vector x of the robot arm 2The centrifugal and Coriolis force matrices; G(x 1 ) is the actual angle vector x based on the robot arm 1 The gravity matrix.

[0101] The direct model generator for the outer loop planner is as follows:

[0102] x 1r =x d

[0103]

[0104] Among them, U d is the optimization result of the direct model equivalent compensation of the control input torque U of the robot arm; 1r and x 2r are the expected trajectory x of the joint angles of the robot arm d and the desired trajectory of the joint angular velocity The optimization result of is the expected trajectory of the joint angular acceleration of the robot; M(x d ) is the expected trajectory x based on the joint angle of the robot arm d The inertia matrix of is the expected trajectory x based on the joint angles of the robot d and the desired trajectory of the joint angular velocity The centrifugal and Coriolis force matrices; G(x d ) is the expected trajectory x based on the joint angle of the robot arm d The gravity matrix.

[0105] The optimal control input of the manipulator output by the direct model generator is the optimal result U of the control input torque U of the manipulator r , the optimized trajectory of the robot output by the direct model generator includes the expected trajectory x of the joint angles of the robot d The optimization result x 1r and the desired trajectory of the joint angular velocity The optimization result x 2r .

[0106] The switch for the outer loop planner is as follows:

[0107]

[0108] e=xx d

[0109]

[0110] Among them, V s() is the triggering Lyapunov function; e is the actual joint angle x and the expected joint angle x of the robot arm. d The tracking error; ξ is the actual joint angular velocity of the robot and the desired joint angular velocity The tracking error of P 1 is the first tracking error z between the inner loop controller and the outer loop planner 1 The weighted matrix norm of ; M is the inertia matrix; K 1 is the sliding surface control gain.

[0111] When the Lyapunov function V is triggered s (e,ξ)>c s When , the optimization generator of the outer loop planner is switched; when the Lyapunov function V is triggered s (e,ξ)≤c s , then switch to the direct model generator of the outer loop planner, c s is the constant threshold of the switch.

[0112] The inner loop controller is an adaptive robust controller based on disturbance estimation. The inner loop controller includes a disturbance estimator and a saturation controller. The optimized trajectory of the manipulator is input into the disturbance estimator. After processing, the disturbance estimator outputs the observation value of the nominal value of the sum of uncertainty and nonlinearity and the robust feedback term to the saturation controller. At the same time, the optimized control input of the manipulator is input into the saturation controller. After processing, the saturation controller outputs the final control input of the manipulator. The inner loop controller is as follows:

[0113] a) Interference Estimator:

[0114]

[0115] z 1 =x 1 -x 1r

[0116]

[0117] in, and are the nominal values ​​of the sum of uncertainty and nonlinearity of the robot arm, respectively. n The observed value and its derivative; Proj() is the projection mapping function; γ d is a positive constant diagonal matrix; z 1 and z 2 are the first and second tracking errors of the inner loop controller and the outer loop observer, respectively; is the first tracking error z between the inner loop controller and the outer loop observer 1 The derivative of K 1 is the sliding surface control gain; x1 and x 2 are the actual angle vector and actual angular velocity vector of the robot arm, respectively, 1r and x 2r are the expected trajectory x of the joint angles of the robot arm d and the desired trajectory of the joint angular velocity The optimization results.

[0118] The disturbance estimator outputs the nominal value d of the sum of uncertainty and nonlinearity of the robot arm at the current moment n The derivative of the observed value Then, the nominal value d of the sum of uncertainty and nonlinearity of the robot arm at the next moment is obtained. n Observed value to the saturation controller.

[0119] The projection mapping function Proj() is as follows:

[0120]

[0121] in,· i is the i-th term of the projection mapping function; (d) i is the nominal value of the sum of uncertainty and nonlinearity of the robot arm d n Observed value The i-th term of di is the boundary value of the i-th term of the uncertainty and nonlinear sum of the robot arm.

[0122] b) Saturation controller:

[0123] U=σ(z)U s +(1-σ(z))U o

[0124]

[0125] U s =[u s1 ,u s1 ,...,u si ,...,u sn ] T

[0126] u si =-sign((z 2 ) i ) imax ,i=1,2,...,n

[0127] U o =U x +dK s z

[0128]

[0129] Among them, U is the final control input of the robot arm, U s and U o are the saturation control law and the adaptive robust control law, respectively, u s1 ,u s2 ,...,u si ,...,u sn are the 1st, 2nd, …, i, …, nth elements in the saturation control law, U imax is the maximum value of the i-th element in the saturation control law, sign() is the sign function; σ() and V() are the judgment function and Lyapunov function respectively; c is a positive constant parameter; P 1 is the first tracking error z between the inner loop controller and the outer loop planner 1 The weighted matrix norm of ; M is the inertia matrix; U x The optimal control input U of the manipulator output by the optimization generator of the outer loop planner r Or the optimized control input U of the robot arm output by the direct model generator d ; K s is the robust feedback term K s The gain matrix of z; Q 2 is the second positive constant.

[0130] Step 3, input the actual trajectory and expected trajectory of the robot arm into the optimization generator of the outer loop planner for processing, and input the actual trajectory of the robot arm into the direct model generator of the outer loop planner for processing. After processing, the optimization generator and the direct model generator both output the optimized control input and optimized trajectory of the robot arm; the switch of the outer loop planner makes a judgment based on the actual trajectory of the robot arm and switches between the optimization generator and the direct model generator. The outer loop planner inputs the optimized control input and optimized trajectory of the robot arm output by the optimization generator or the direct model generator selected by the switch into the inner loop controller for processing. After processing, the inner loop controller outputs the final control input of the robot arm, thereby performing integrated planning and control of the robot arm.

[0131] In order to verify the effectiveness and superiority of the method of the present invention, the following control methods are simulated and compared, and the specific embodiments are as follows:

[0132] C1: Reference governor. The inner loop controller is a proportional-derivative PD controller based on model compensation. The details are as follows:

[0133] U=U c +U s

[0134]

[0135] v(t)=v(t-1)+κ(q d -v(t-1))

[0136] Among them, U is the total control input of the robot arm, U c is the model compensation term, U s is the PD control item; K p is the proportional control parameter matrix, K d is the differential control parameter matrix; x 1 is the actual joint angle of the robot arm, x 2 is the actual joint angular velocity of the robot; v(t) and v(t-1) are the preset control inputs of the robot at time t and time t-1, respectively. The first-order derivative of the preset control input of the robot arm; q d is the desired trajectory of the robot arm joint angle; κ presets the optimization parameter of the control input. Set the control parameters as: K p =[200,200] T ,K d =[50,50] T .

[0137] C2: The integrated planning control method based on nonlinear optimization proposed in this invention sets the control parameters as follows: Q 1 =diag([500,500]),P 1 =diag([50,50]),K 1 =diag([10,10]),Q 2 =diag([50,10]), δ=[1.5,0.5] T , ε=[3,0.5] T ,η=[5,1.5] T , γ d =diag([1000,200]),d max =[3,5] T , c=0.02, c s =0.019.

[0138] The robot model used for simulation is set to a two-degree-of-freedom robot, and the dynamics-related matrix is:

[0139]

[0140] M 11 =(m 1 +m 2 ) 12 +m 2 l 2 2 +2m 2 l 1 l 2 cos(q 2 ),

[0141] M 12 =M 21 =m 2 l 2 2 +m 2 l 1 l 2 cos(q 2 ),

[0142] M 22 =m 2 l 2 2 ,

[0143]

[0144] C 22 =0,

[0145] G 1 =(m 1 +m 2 ) 1 cos(q 2 )+m 2 l 2 cos(q 1 +q 2 )g,

[0146] G 2 =m 2 l 2 cos(q 1 +q 2 )g,

[0147] 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 Elements in G 1 and G 2 are the elements of the gravity matrix G(q) of the two-degree-of-freedom manipulator; m 1 and m2 are the masses of the first and second robotic arms, respectively, l 1 and l 2 are the lengths of the first and second robotic arms respectively. In specific implementation, m 1 =2kg,m 2 =2kg,l 1 =0.42m,l 2 =0.42m; g is the acceleration due to gravity.

[0148] The state constraints and input constraints of the robotic arm system in the above simulation experiment are set as follows:

[0149] -2.8973≤q 1 ≤2.8973,-1.7628≤q 2 ≤1.7628,

[0150]

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

[0152] like Figure 2 (a) Figure 2 (b) Figure 3 (a) and Figure 3 As shown in (b), the convergence time of the PD-based reference regulator is about 1.16s, and the angle tracking error of the first joint is 10 -3 Level, the angle tracking error of the second joint is 10 -2 level; the convergence time of the integrated planning control method based on nonlinear optimization proposed in the present invention is about 0.6s, and the angle tracking errors of the first joint and the second joint are both 10 -5 It can be concluded that the integrated planning control method based on nonlinear optimization proposed in the present invention has a faster convergence speed and higher steady-state tracking accuracy than the reference regulator based on PD.

[0153] like Figure 4 (a) Figure 4 (b) Figure 5 (a) and Figure 5 As shown in (b), the integrated planning control method based on nonlinear optimization proposed in the present invention satisfies both state and input constraints, while the PD-based reference regulator violates the constraints at the constraint boundary. It can be concluded that the integrated planning control method based on nonlinear optimization proposed in the present invention can comply with the constraints more strictly than the PD-based reference regulator.

[0154] 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 robot arm integrated planning and control method based on nonlinear optimization, which is characterized by comprising: Step 1, establishing a nonlinear dynamic state space model of the manipulator under state constraints and input constraints; Step 2, establishing a planning and control integrated dual-loop control model based on the nonlinear dynamic state space model of the robot arm, wherein the planning and control integrated dual-loop control model includes an outer loop planner and an inner loop controller, wherein the outer loop planner includes an optimization generator, a direct model generator, and a switcher; Step 3, input the actual trajectory and expected trajectory of the robot arm into the optimization generator of the outer loop planner for processing, and input the actual trajectory of the robot arm into the direct model generator of the outer loop planner for processing. After processing, the optimization generator and the direct model generator both output the optimized control input and optimized trajectory of the robot arm; the switch of the outer loop planner makes a judgment based on the actual trajectory of the robot arm and switches between the optimization generator and the direct model generator. The outer loop planner inputs the optimized control input and optimized trajectory of the robot arm output by the optimization generator or the direct model generator selected by the switch into the inner loop controller for processing. After processing, the inner loop controller outputs the final control input of the robot arm, thereby performing integrated planning and control of the robot arm.

2. The integrated planning and control method of a manipulator based on nonlinear optimization according to claim 1 is characterized in that: in the step 1, the nonlinear dynamic state space model of the manipulator is specifically as follows: in, x1 and are the first state variable and its derivative, i.e., the actual angle vector and actual angular velocity vector of the robot, x2 and are the second state variable and its derivative, i.e., the actual angular velocity vector and the actual angular acceleration vector of the robot arm; M(x1) is the inertia matrix of the robot arm based on the first state variable x1; U is the control input torque of the robot arm; C(x1,x2) is the centrifugal force and Coriolis force matrix of the robot arm based on the first state variable x1 and the second state variable x2; G(x1) is the gravity matrix of the robot arm based on the first state variable x1; Δ is the sum of uncertainty and nonlinearity of the robot arm; The state constraints of the nonlinear dynamic state space model of the robot are as follows: x1∈[x 1min ,x 1max ] x2∈[x 2min ,x 2max ] Among them, x 1max and x 1min are the upper and lower boundary vectors of the first state variable x1; 2max and x 2min are the upper and lower boundary vectors of the second state variable x2 respectively; The input constraints of the nonlinear dynamic state space model of the manipulator are as follows: U∈[-U max ,IN max ] Among them, U max It is the maximum value vector of the control input torque U of the robot arm.

3. The integrated planning and control method of a manipulator based on nonlinear optimization according to claim 1 is characterized in that: in the step 2, the optimization generator of the outer loop planner is specifically as follows: K′=[K1,-I] in, U r is the optimization result of the control input torque U of the robot arm; x 1r and are the expected trajectory x of the joint angles of the robot arm d The optimization results and their derivatives, x 2r and are the expected trajectories of the joint angular velocities of the robot arm The optimization results and their derivatives of T f is the prediction time length, t0 is the initial time of the sampling period; M(x 1r ) is the expected trajectory x based on the joint angle of the robot arm d The optimization result x 1r The inertia matrix of 1r ,x 2r ) is the expected trajectory x based on the joint angle of the robot arm d The optimization result x 1r and the desired trajectory of the joint angular velocity The optimization result x 2r The centrifugal and Coriolis force matrices; G(x 1r ) is the expected trajectory x based on the joint angle of the robot arm d The optimization result x 1r The gravity matrix of x 1r (t0) and x 2r (t0) are the expected trajectory x of the joint angle of the robot at time t0 d and the desired trajectory of the joint angular velocity The optimization result of the robot arm is: x1(t0) and x2(t0) are the actual trajectories of the joint angle and joint angular velocity of the robot arm at time t0; x 1r (t0+T f ) and x 2r (t0+T f ) are t0+T f The expected trajectory of the joint angle of the robot arm at time x d and the desired trajectory of the joint angular velocity The optimization result of x d (t0+T f )and t0+T f The expected trajectory of the joint angle and angular velocity of the robot arm at the moment; U max is the maximum value vector of the control input torque U of the robot; η is the model compensation error scaling factor matrix, ε is the feedback term scaling factor matrix, δ d is the scaling factor for interference compensation; (x 1min ) i and (x 1max ) i are the expected trajectory x of the joint angles of the robot arm d The optimization result x 1r The minimum state constraint matrix x 1min and the maximum state constraint matrix x 1max The i-th row element in (Δx 1min ) i and (Δx 1max ) i are the expected trajectory x of the joint angles of the robot arm d The optimization result x 1r The minimum state constraint matrix x 1min The scaling factor matrix Δx 1min and the maximum state constraint matrix x 1max The scaling factor matrix Δx 1max The i-th row element in (x 2min ) i and (x 2max ) i are the expected trajectories of the joint angular velocities of the robot arm The optimization result x 2r The minimum state constraint matrix x 2min and the maximum state constraint matrix x 2max The i-th row element of (Δx 2min ) i and (x 2max ) i are the expected trajectories of the joint angular velocities of the robot arm The optimization result x 2r The minimum state constraint matrix x 2min The scaling factor matrix Δx 2min and the maximum state constraint matrix x 2max The scaling factor matrix Δx 2max The i-th row element of ; c is a positive constant parameter; max() is the maximum value function; (I) i is the i-th row element of the identity matrix I; P′ is the first weighted matrix norm of the scaling factor matrix of the state constraint matrix, diag[ ] is a diagonal matrix, P1 is the weighted matrix norm of the first tracking error z1 of the inner loop controller and the outer loop planner, and M is the inertia matrix; (K') i is the i-th row element of the second weighted matrix norm K' of the scaling factor matrix of the state constraint matrix, and K1 is the sliding surface control gain; (A) i is the i-th row element of the first positive constant-valued diagonal matrix A; η i and ε i are the i-th element in the model compensation error scaling factor matrix η and the feedback term scaling factor matrix ε respectively; (K s ) i is the gain matrix K of the robust feedback term s The i-th row element of ; The optimal control input of the robot arm output by the optimization generator is the optimization result U of the control input torque U of the robot arm. r , the optimized trajectory of the robot output by the optimization generator includes the expected trajectory x of the joint angles of the robot d The optimization result x 1r and the desired trajectory of the joint angular velocity The optimization result x 2r .

4. The integrated planning and control method of a manipulator based on nonlinear optimization according to claim 3 is characterized in that: the positive first constant diagonal matrix A related to the model compensation error satisfies the following constraints: in, The model compensation error matrix is The i-th row element of is; z is the total tracking error vector composed of the first tracking error z1 and the second tracking error z2 of the inner loop controller and the outer loop planner, z=[z1,z2] T ; (Az) i is the i-th row element of the multiplication matrix Az of the positive first constant diagonal matrix A and the total tracking error vector z; M(x1) is the inertia matrix based on the actual angle vector x1 of the robot arm; C(x1,x2) is the centrifugal force and Coriolis force matrix based on the actual angle vector x1 of the robot arm and the actual angular velocity vector x2 of the robot arm; G(x1) is the gravity matrix based on the actual angle vector x1 of the robot arm.

5. The integrated planning and control method of a manipulator based on nonlinear optimization according to claim 1 is characterized in that: in the step 2, the direct model generator of the outer loop planner is specifically as follows: x 1r =x d in, U d is the optimization result of the control input torque U of the robot arm; x 1r and x 2r are the expected trajectory x of the joint angles of the robot arm d and the desired trajectory of the joint angular velocity The optimization result of is the expected trajectory of the joint angular acceleration of the robot; M(x d ) is the expected trajectory x based on the joint angle of the robot arm d The inertia matrix of is the expected trajectory x based on the joint angles of the robot arm d and the desired trajectory of the joint angular velocity The centrifugal and Coriolis force matrices; G(x d ) is the expected trajectory x based on the joint angle of the robot arm d The gravity matrix of The optimal control input of the manipulator output by the direct model generator is the optimal result U of the control input torque U of the manipulator r , the optimized trajectory of the robot output by the direct model generator includes the expected trajectory x of the joint angles of the robot d The optimization result x 1r and the desired trajectory of the joint angular velocity The optimization result x 2r .

6. The integrated planning and control method of a manipulator based on nonlinear optimization according to claim 1 is characterized in that: in the step 2, the switch of the outer loop planner is specifically as follows: e=xx d in, V s ( ) is the triggering Lyapunov function; e is the actual joint angle x and the expected joint angle x of the robot arm d The tracking error; ξ is the actual joint angular velocity of the robot and the desired joint angular velocity The tracking error; P1 is the weighted matrix norm of the first tracking error z1 of the inner loop controller and the outer loop planner; M is the inertia matrix; K1 is the sliding surface control gain; When the Lyapunov function V is triggered s (e,ξ)>c s , then switch to the optimization generator of the outer loop planner; When the Lyapunov function V is triggered s (e,ξ)≤c s , then switch to the direct model generator of the outer loop planner, c s is the constant threshold of the switch.

7. According to the nonlinear optimization-based integrated planning and control method of the manipulator in claim 1, the characteristic is that: in the step 2, the inner loop controller includes a disturbance estimator and a saturation controller, the optimized trajectory of the manipulator is input into the disturbance estimator, and the disturbance estimator outputs the observed value of the nominal value of the sum of uncertainty and nonlinearity and the robust feedback term to the saturation controller after processing, and the optimized control input of the manipulator is input into the saturation controller again, and the saturation controller outputs the final control input of the manipulator after processing, and the inner loop controller is specifically as follows: a) Interference Estimator: z1=x1-x 1r in, and are the nominal values ​​of the sum of uncertainty and nonlinearity of the robot arm, respectively. n The observed value and its derivative; Proj( ) is the projection mapping function; γ d is a positive constant diagonal matrix; z1 and z2 are the first and second tracking errors of the inner loop controller and the outer loop observer respectively; is the derivative of the first tracking error z1 of the inner loop controller and the outer loop observer; K1 is the sliding surface control gain; x1 and x2 are the actual angle vector and actual angular velocity vector of the robot arm, respectively. 1r and x 2r are the expected trajectory x of the joint angles of the robot arm d and the desired trajectory of the joint angular velocity The optimization results of The disturbance estimator outputs the nominal value d of the sum of uncertainty and nonlinearity of the robot arm at the current moment n The derivative of the observed value Then, the nominal value d of the sum of uncertainty and nonlinearity of the robot arm at the next moment is obtained. n Observed value to the saturation controller; b) Saturation controller: U=σ(z)U s +(1-σ(z))U o IN s =[in s1 ,in s1 ,...,in si ,...,in sn ] T u si =-sign((z2) i )U imax ,i=1,2,...,n Among them, U is the final control input of the robot arm, U s and U o are the saturation control law and the adaptive robust control law, respectively, u s1 ,u s2 ,...,u si ,...,u sn are the 1st, 2nd, …, i, …, nth elements in the saturation control law, U imax is the maximum value of the i-th element in the saturation control law, sign( ) is the sign function; σ( ) and V( ) are the judgment function and Lyapunov function respectively; c is a positive constant parameter; P1 is the weighted matrix norm of the first tracking error z1 of the inner loop controller and the outer loop planner; M is the inertia matrix; U x The optimal control input U of the manipulator output by the optimization generator of the outer loop planner r Or the optimized control input U of the robot arm output by the direct model generator d ; K s is the robust feedback term K s The gain matrix of z; Q2 is the positive second constant diagonal matrix.

8. An electronic device, 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 7.

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

Citation Information

Patent Citations

  • Mobile mechanical arm robust control method and system based on non-linear disturbance observer

    CN112643673A

  • Double-loop high-performance control method based on expected trajectory limited optimization

    CN115128951A

  • Limited self-adaptive robust control method and system for two-degree-of-freedom mechanical arm

    CN115338871A

  • Adaptive robust control device

    US5959861A

Cited By

  • Online optimal control method for nonlinear MIMO (Multiple Input Multiple Output) system based on approximate dynamic programming

    CN121785149A