Multi-objective optimization RRT* robot path planning method based on cooperative game
Through the dual closed-loop disturbance observation MPC algorithm, the problems of model mismatch and external disturbance in collaborative robot trajectory tracking are solved, and high-precision and robust trajectory tracking control is achieved.
Patent Information
- Application Number
- CN202510934917.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-07-08
- Publication Date
- 2025-10-14
AI Technical Summary
The existing model predictive control (MPC) in collaborative robot trajectory tracking has problems such as large computational complexity, model mismatch and low tracking accuracy caused by external disturbances, making it difficult to meet the requirements of high precision and robustness.
A dual closed-loop disturbance observation MPC algorithm is adopted. By establishing the robot's linear discrete state space equations and nonlinear disturbance observer, auxiliary inner and outer loop state equations are designed, and the dual closed-loop MPC online calculation expression is derived to enhance the anti-disturbance ability and robustness.
It effectively compensates for the influence of the robot's dynamic model mismatch, improves the accuracy and robustness of trajectory tracking control, and enhances resistance to external disturbances.
Smart Images
Figure CN120779953A_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the technical field of robot control, and in particular relates to a collaborative robot trajectory tracking control algorithm based on dual closed-loop disturbance observation MPC. Background Art
[0002] With the rapid development of emerging technologies such as artificial intelligence and the Internet of Things, the global manufacturing industry is undergoing a profound intelligent transformation. This transformation not only improves industrial efficiency but also reshapes the structure and model of traditional manufacturing systems, driving the manufacturing industry towards intelligence, automation, and flexibility. Countries are continuously launching intelligent manufacturing development strategies to enhance manufacturing automation and maximize production efficiency. Against this backdrop, robotics technology is rapidly developing and becoming a key support for intelligent manufacturing. Collaborative robots, a new type of industrial robot that can work collaboratively with humans during tasks, are highly aligned with the core concepts of today's intelligent development. Therefore, the development of collaborative robots has become a key driver of intelligent manufacturing.
[0003] Model Predictive Control (MPC) is an advanced control strategy based on dynamic models and rolling optimization. It is suitable for processing complex nonlinear time-varying systems with multiple variables and multiple constraints. It can reduce the impact of model mismatch through feedback correction and rolling optimization, and is therefore widely used in industrial control.
[0004] Because collaborative robots operate in a shared environment with human-robot collaboration, they are not only required to have high precision and responsiveness, but also to consider special requirements such as obstacle avoidance and compliant contact. For these scenarios, input control variables, as well as state and output variables, must be constrained. Meeting all dynamic constraints is a significant challenge for classic control schemes. Compared to sliding film control and robust control, MPC is better at solving trajectory tracking problems for high-dimensional systems and can meet various constraints, with good stability and robustness. However, it still suffers from high computational complexity, low tracking accuracy due to model mismatch and external disturbances, and therefore, MPC is a control strategy of great research value in the field of robotics. Summary of the Invention
[0005] To solve the above technical problems, the present invention provides a collaborative robot trajectory tracking control algorithm based on dual closed-loop disturbance observation MPC, which can effectively compensate for the impact of robot dynamic model mismatch on control, improve algorithm robustness, and enhance anti-disturbance capability.
[0006] The technical solution adopted by the present invention is: a collaborative robot trajectory tracking control algorithm based on dual closed-loop disturbance observation MPC, the specific steps are as follows:
[0007] S1. Establish the linear discrete state space equation of the robot;
[0008] The robot dynamic model is established as:
[0009]
[0010] Define the state vector Input vector Output vector
[0011] Then the robot state space equation expression is:
[0012]
[0013] Where f(x1,x2)=-H(x1) -1 (C(x1,x2)x2+G(x1)).
[0014] Define a new control variable v that satisfies:
[0015] τ=H(x1)(vf(x1,x2)) (3)
[0016] By performing Taylor expansion on the state equation at each moment, the discrete linearized state equation of the robot system is obtained as follows:
[0017]
[0018] S2. Establish a nonlinear disturbance observer;
[0019] The nonlinear disturbance observer is defined as:
[0020]
[0021] Construct auxiliary variables:
[0022]
[0023] In order for the auxiliary variable to replace the acceleration term, it should satisfy the following conditions:
[0024]
[0025] Substituting Equation (7) and Equation (6) into Equation (5), the improved nonlinear disturbance observer can be obtained as follows:
[0026]
[0027] design for:
[0028]
[0029] The derivative of the above formula is substituted into formula (7) to obtain the nonlinear function The calculation formula of the nonlinear function is
[0030]
[0031] S3, derive the double closed-loop MPC online calculation expression;
[0032] Define the auxiliary inner loop control variable c(k) as
[0033] v(k) = v(x(k), c(k)) = Px(k) + p c c(k) (11)
[0034] where P = [p1I n×n p2I n×n ] n×2n , p1, p2 and p c represent the weights of state variables x1, x2 and inner loop control variable c in control variable v.
[0035] The new state equation is obtained as
[0036] x(k+1) = A au x(k) + B au c(k) + w(k) (12)
[0037] where:
[0038]
[0039] The bounded disturbance w(k) is usually difficult to establish an accurate model, and the calculated value of x(k+1) affected by the disturbance is different from the actual expected value, and the influence of the disturbance on the state variable will be amplified in the iteration. Therefore, a new outer loop system is constructed to track the expected state variable, and the outer loop state equation is defined as
[0040]
[0041] and The expression of and
[0042]
[0043] The outer loop state equation is simplified as
[0044]
[0045] where:
[0046]
[0047] Define from the kth moment to the k+Nc The prediction auxiliary inner loop control variables at time -1 are:
[0048]
[0049] Define from the kth moment to the k+Nth moment p The predicted new state variable at time -1 is:
[0050]
[0051] Iterating the state space equation yields:
[0052]
[0053] in:
[0054]
[0055] in, i a1 represents the value of a1 in the state equation at the i-th iteration, i=1,2,…N p -1, and the same applies below.
[0056]
[0057]
[0058] Where j = N c ,N c +1,…,N p -1
[0059] The outer loop state variable z(k) needs to track the state variable x(k), so the new performance indicator function is defined as:
[0060]
[0061] Among them, X d (k) represents the time from the kth moment to the k+Nth moment p The expected state at time -1.
[0062] and They represent the weight matrices of state error and auxiliary control limit in the optimization process, respectively, and their expressions are:
[0063]
[0064] in, and They represent the weights of angle error, velocity error and control limit at the k+i moment respectively.
[0065]
[0066]
[0067] Since the outer loop state variable z is a trace of x, it can be considered that z1(k)=x1(k) and z2(k)=x2(k) during calculation.
[0068] The analytical expression of the optimal auxiliary control quantity is:
[0069]
[0070] in:
[0071]
[0072] The expression of dual closed-loop MPC optimal torque control is obtained as follows:
[0073]
[0074] Substituting the disturbance estimate obtained by the nonlinear disturbance observer into equation (30), the expression of the optimal torque control of the double closed-loop disturbance observer MPC is obtained as follows:
[0075] BRIEF DESCRIPTION OF THE DRAWINGS
[0076] Figure 1 This is a control block diagram of a collaborative robot trajectory tracking control algorithm based on dual closed-loop disturbance observation MPC of the present invention. DETAILED DESCRIPTION
[0077] The method of the present invention is further described below with reference to the accompanying drawings and embodiments.
[0078] like Figure 1 As shown in FIG, a collaborative robot trajectory tracking control algorithm based on dual closed-loop disturbance observation MPC of the present invention has the following specific steps:
[0079] S1. Establish the linear discrete state space equation of the robot;
[0080] The robot dynamic model is established as:
[0081]
[0082] Define the state vector Input vector Output vector Then the robot state space equation expression is:
[0083]
[0084] Where f(x1,x2)=-H(x1) -1 (C(x1,x2)x2+G(x1)).
[0085] Define a new control variable v that satisfies:
[0086] τ=H(x1)(vf(x1,x2)) (3)
[0087] By performing Taylor expansion on the state equation at each moment, the discrete linearized state equation of the robot system is obtained as follows:
[0088]
[0089] S2. Establish a nonlinear disturbance observer;
[0090] The nonlinear disturbance observer is defined as:
[0091]
[0092] Construct auxiliary variables:
[0093]
[0094] In order for the auxiliary variable to replace the acceleration term, it should satisfy the following conditions:
[0095]
[0096] Substituting Equation (7) and Equation (6) into Equation (5), the improved nonlinear disturbance observer can be obtained as follows:
[0097]
[0098] design for:
[0099]
[0100] After derivation of the above formula and inserting it into formula (7), we can get the nonlinear function The calculation formula is:
[0101]
[0102] S3. Derive the online calculation expression of dual closed-loop MPC;
[0103] Define the auxiliary inner loop control variable c(k) as:
[0104] v(k)=v(x(k),c(k))=Px(k)+p c c(k) (11)
[0105] Where P = [p1In×n p2I n×n ] n×2n , p1, p2 and p c Represents the weights of state variables x1, x2 and inner loop control variable c in the control variable v.
[0106] The new state equation is:
[0107] x(k+1)=A au x(k)+B au c(k)+w(k) (12)
[0108] in:
[0109]
[0110] It is often difficult to accurately model bounded perturbations w(k). The calculated value of x(k+1) affected by the perturbation differs from the actual expectation, and the impact of the perturbation on the state variables is amplified during iteration. Therefore, a new outer loop system is constructed to track the desired state variables, and the outer loop state equation is defined as:
[0111]
[0112] and The expression is:
[0113]
[0114] The outer loop state equation is simplified as follows:
[0115]
[0116] in:
[0117]
[0118] Define from the kth moment to the k+Nth moment c The prediction auxiliary inner loop control variables at time -1 are:
[0119]
[0120] Define from the kth moment to the k+Nth moment p The predicted new state variable at time -1 is:
[0121]
[0122] Iterating the state space equation yields:
[0123]
[0124] in:
[0125]
[0126] in, i a1 represents the value of a1 in the state equation at the i-th iteration, i=1,2,…N p -1, and the same applies below.
[0127]
[0128]
[0129] Where j = N c ,N c +1,…,N p -1
[0130] The outer loop state variable z(k) needs to track the state variable x(k), so the new performance indicator function is defined as:
[0131]
[0132] Among them, X d (k) represents the time from the kth moment to the k+Nth moment p The expected state at time -1.
[0133] and They represent the weight matrices of state error and auxiliary control limit in the optimization process, respectively, and their expressions are:
[0134]
[0135] in, and They represent the weights of angle error, speed error and control limit at the k+i moment respectively.
[0136]
[0137]
[0138] Since the outer loop state variable z is a trace of x, it can be considered that z1(k)=x1(k) and z2(k)=x2(k) during calculation.
[0139] The analytical expression of the optimal auxiliary control quantity is:
[0140]
[0141] in:
[0142]
[0143] The expression of dual closed-loop MPC optimal torque control is obtained as follows:
[0144]
[0145] Substituting the disturbance estimate obtained by the nonlinear disturbance observer into equation (30), the expression of the dual closed-loop disturbance observer MPC optimal torque control is obtained as follows:
[0146]
[0147] MPC is an advanced control strategy based on dynamic models and rolling optimization. It is suitable for processing complex nonlinear time-varying systems with multiple variables and multiple constraints. It can also reduce the impact of model mismatch through feedback correction and rolling optimization. Therefore, it is widely used in industrial control. The method of the present invention obtains a simplified linear discrete state space equation through feedback linearization and Taylor expansion, and then obtains a nonlinear disturbance observer to estimate the disturbance torque based on the feedback difference. At the same time, auxiliary control variables are introduced to obtain a new state equation, and a dual closed-loop MPC optimal control expression is derived. The method of the present invention effectively compensates for the impact of the robot dynamic model mismatch on the control, improves the robustness of the algorithm, and enhances the anti-disturbance capability.
[0148] Those skilled in the art will appreciate that the embodiments described herein are intended to help readers understand the principles of the present invention, and it should be understood that the scope of protection of the present invention is not limited to such specific descriptions and embodiments. Those skilled in the art can make various other specific variations and combinations based on the technical teachings disclosed in the present invention without departing from the essence of the present invention, and such variations and combinations are still within the scope of protection of the present invention.
Claims
1. The collaborative robot trajectory tracking control algorithm based on dual closed-loop disturbance observation MPC is as follows: S1. Establish the linear discrete state space equation of the robot; The robot dynamic model is established as: Define the state vector Input vector Output vector Then the robot state space equation expression is: in, f(x1,x2)=-H(x1) -1 (C(x1,x2)x2+G(x1))。 Define a new control variable v that satisfies: τ=H(x1)(vf(x1,x2)) (3) By performing Taylor expansion on the state equation at each moment, the discrete linearized state equation of the robot system is obtained as follows: S2. Establish a nonlinear disturbance observer; The nonlinear disturbance observer is defined as: Construct auxiliary variables: In order for the auxiliary variable to replace the acceleration term, it should satisfy the following conditions: Substituting Equation (7) and Equation (6) into Equation (5), the improved nonlinear disturbance observer can be obtained as follows: design for: After derivation of the above formula and inserting it into formula (7), we can get the nonlinear function The calculation formula is: S3. Derive the online calculation expression of dual closed-loop MPC; Define the auxiliary inner loop control variable c(k) as: v(k)=v(x(k),c(k))=Px(k)+p c c(k) (11) Where P = [p1I n×n p2I n×n ] n×2n , p1, p2 and p c Represents the weights of state variables x1, x2 and inner loop control variable c in the control variable v. The new state equation is: x(k+1)=A au x(k)+B au c(k)+w(k) (12) in: It is often difficult to accurately model bounded perturbations w(k). The calculated value of x(k+1) affected by the perturbation differs from the actual expectation, and the impact of the perturbation on the state variables is amplified during iteration. Therefore, a new outer loop system is constructed to track the desired state variables, and the outer loop state equation is defined as: and The expression is: The outer loop state equation is simplified as follows: in: Define from the kth moment to the k+Nth moment c The prediction auxiliary inner loop control variables at time -1 are: Define from the kth moment to the k+Nth moment p The predicted new state variable at time -1 is: Iterating the state space equation yields: in: in, i a1 represents the value of a1 in the state equation at the i-th iteration, i=1,2,…N p -1, and the same applies below. Where j = N c ,N c +1,…,N p -1 The outer loop state variable z(k) needs to track the state variable x(k), so the new performance indicator function is defined as: Among them, X d (k) represents the time from the kth moment to the k+Nth moment p The expected state at time -1. and They represent the weight matrices of state error and auxiliary control limit in the optimization process, respectively, and their expressions are: in, and They represent the weights of angle error, velocity error and control limit at the k+i moment respectively. Since the outer loop state variable z is a trace of x, it can be considered that z1(k)=x1(k) and z2(k)=x2(k) during calculation. The analytical expression of the optimal auxiliary control quantity is: in: The expression of dual closed-loop MPC optimal torque control is obtained as follows: Substituting the disturbance estimate obtained by the nonlinear disturbance observer into equation (30), the expression of the dual closed-loop disturbance observer MPC optimal torque control is obtained as follows: