A flexible joint model predictive control method considering uncertainty and multiple constraints

Abstract

This invention provides a model predictive control method for flexible joints that considers uncertainties and multiple constraints, belonging to the field of flexible joint drive control. The method includes: within the framework of model predictive control, describing constraints such as joint obstacle avoidance, saturation, and velocity as inequality constraints under an optimal control problem, thus obtaining an optimal control law for the multi-constrained flexible joint drive system in the form of a linear quadratic programming problem; designing a discrete-form unknown state estimator to estimate uncertain disturbances in the joint when only the nominal values ​​of joint inertia and stiffness parameters are known; and designing a constraint-adaptive hierarchical programming method to dynamically adjust the priority of constraints such as obstacle avoidance and saturation according to changes in the joint's motion environment, improving the solvability of the control problem while satisfying as many constraints as possible. This method simultaneously considers the impact of uncertain disturbances and multiple constraint conflicts on flexible joint control, enabling the flexible joint to achieve trajectory tracking and autonomous obstacle avoidance under the influence of uncertain disturbances.

Description

Technical Field

[0001] This invention belongs to the field of flexible joint drive control, specifically relating to a model predictive control method for flexible joints that considers uncertainties and multiple constraints. Background Technology

[0002] Flexible joint robots are widely used in various fields due to their unique joint flexibility, which places higher demands on the environmental adaptability and control accuracy of the joint drive system. Model-based control methods are considered an effective way to improve joint control accuracy, but building accurate models is difficult. Joint hardware parameter errors, nonlinear modal variations, and external unknown disturbances can all affect modeling accuracy, thereby reducing joint control performance. Flexible joints are not only affected by various external factors in applications, but also need to satisfy multiple constraints in joint drive control, increasing the difficulty of solving joint control laws and even leading to situations where the joint control law is unsolvable.

[0003] Model Predictive Control (MPC) has significant advantages in handling constrained optimization problems. It transforms external physical constraints into optimal control problems with inequality constraints through quadratic programming, thereby improving the solvability of constrained problems. However, when constraints increase or potential constraint conflicts exist, existing MPC methods still suffer from unsolvable control laws or unsatisfied hard constraints. Setting constraint priorities can resolve constraint conflicts; for example, mixed-integer model predictive control methods design constraint priority programming based on logical variables, mitigating the impact of constraint conflicts. However, this often defaults to optimizing multiple constraints at the same level, weakening the role of hard constraints such as saturation and obstacle avoidance, and the controllers designed do not consider the effects of uncertain disturbances, resulting in poor robustness. Various observers are considered effective tools for solving disturbance problems, such as state observers, extended state observers (ESOs), and unscented Kalman filters (UKFs). However, existing observers suffer from problems such as reliance on full-state measurements, the need to calculate the inverse of the parameter matrix, and dependence on formally accurate dynamic models. The Unknown State Estimator (USE) is an estimator that does not rely on acceleration feedback and has lower model dependence. However, its form is only applicable to disturbance estimation of continuous-time systems and cannot be applied to discrete prediction models of model predictive control. Summary of the Invention

[0004] To address the issues of low control accuracy and potential unsolvable control laws in flexible joints when both uncertain disturbances and multiple constraints coexist, this invention provides a model predictive control method for flexible joints that considers uncertainties and multiple constraints.

[0005] To achieve the above objectives, the present invention provides the following technical solution:

[0006] A predictive control method for flexible joint models considering uncertainties and multiple constraints includes the following steps:

[0007] A flexible joint dynamic model including lumped perturbation is constructed and multiple constraints are set. Based on the flexible joint dynamic model, joint motion state data and model nominal values ​​are obtained.

[0008] The joint motion state and the model nominal value are input into the unknown state estimator to obtain the estimated value of the lumped disturbance;

[0009] A prediction model is constructed by inputting the joint motion state data and the estimated value of the lumped disturbance into the prediction model to construct a flexible joint multi-constraint optimization control problem. The flexible joint multi-constraint optimization control problem is transformed into a quadratic programming problem, and slack variables and priority matrices are introduced to perform adaptive hierarchical programming on the multi-constraint conditions to obtain the reconstructed objective function. The optimal predictive control increment is obtained by solving the reconstructed objective function, and the optimal control increment is used as the input signal of the drive motor to control the joint motion.

[0010] Preferably, the flexible joint dynamic model is as follows:

[0011] ;

[0012] in, This refers to the output angle of the reducer; The angle output by the joint; The moment of inertia of the motor rotor and output shaft; The moment of inertia of the joint, including the reducer and the elastic mechanism; This refers to the joint stiffness coefficient; and These are the length and mass of the arm linkage, respectively. It is the acceleration due to gravity; This refers to the joint driving torque; and These are the frictional torques on the arm linkage side and the motor side, respectively. and It is an unknown external disturbance;

[0013] The sum of the nonlinear terms and external disturbances in the dynamic model of the flexible joint is considered as a lumped disturbance, which is specifically:

[0014] ;

[0015] in, For the lumped disturbance at the arm end, The lumped disturbance at the joint drive motor end. It is the increase in joint inertia. Joint stiffness increment, Increment of rotational inertia of the motor rotor and output shaft angular acceleration of the decelerator, This is the joint angular acceleration.

[0016] Preferably, the multiple constraints specifically include the driving torque constraint of the joint drive motor, the control increment constraint of the joint drive motor, the joint angle constraint, the joint speed constraint, and the obstacle avoidance constraint.

[0017] The specific driving torque constraint of the joint drive motor is as follows:

[0018] ;

[0019] The incremental constraint for the joint drive motor control is specifically as follows:

[0020] ;

[0021] The joint angle constraint is specifically as follows:

[0022] ;

[0023] The joint velocity constraint is specifically as follows:

[0024] ;

[0025] in, Input control quantity for the driving torque of the joint drive motor, Controlling increments Joint angle, Joint velocity;

[0026] The obstacle avoidance constraints are specifically as follows:

[0027] ;

[0028] in, for The inertial velocity at the point; Joint angular velocity; Jacobian matrix transpose, The minimum distance between two points; Indicates the direction of a vector between two points. The minimum safe distance between the target and the obstacle; The distance from the target to the obstacle is the influence distance.

[0029] Preferably, the reconstructed objective function is specifically:

[0030] ;

[0031] ;

[0032] in, , ; Let be the state variable matrix of the joint system, where These are the rotation angle and velocity output by the joint, respectively. These are the rotation angle and speed output by the reducer, respectively; It is the identity matrix; , , , , , These are all coefficient matrices composed of joint dynamic parameters, which are related to the system model; , are the output matrices composed of the joint angles and velocities of each cycle; Input quantity for joint control; This represents the lumped disturbance matrix of the joint drive system, which consists of uncertain disturbances such as unmodeled dynamics and unknown disturbances. For the lumped disturbance at the arm end, The lumped disturbance at the joint drive motor end; These are the maximum and minimum values ​​of the corresponding variables, used to describe the constraint requirements; , , The coefficient matrices of the obstacle avoidance constraints, the minimum safe distance involved in obstacle avoidance, and the minimum safe distance involved in obstacle avoidance are used to describe the obstacle avoidance constraint requirements; To control the increment; For slack variables The constructed matrix divides all constraints into Level, of which the first Level includes There are several constraints, where j = 1, 2, ... ; This is the slack variable weight matrix; The matrix that forms the upper bounds of each relaxation variable; It is a matrix in which all elements except the diagonal elements are 0. The diagonal elements are either 0 or 1, which indicates that the corresponding constraint is a hard constraint or a soft constraint. Let be a matrix of logical variables, where each logical variable satisfies ; This is the priority matrix; To constrain the convergence coefficients, satisfying ; This is the transpose of the slack variable; It is a Hessian matrix. Gradient vector matrix The transpose of .

[0033] Preferably, the unknown state estimator is specifically:

[0034] ;

[0035] in, This represents the nominal value of joint stiffness. This is the nominal value of the motor's end inertia. This represents the nominal value of the moment of inertia of the articulated arm. These are the filter coefficients of the low-pass filter; , , , , State variables , , , as well as The filter variable, For the rotation angle on the side of the joint drive motor, For the speed of the joint drive motor side, For the angle of the joint on the side of the arm, Rotation speed of the joint on the arm side, For driving torque; The sampling period; Sampling time; , This represents the lumped disturbance estimate obtained for each sampling period.

[0036] Preferably, the error of the estimated value calculated by the unknown state estimator is bounded, and it is assumed that there is an upper bound on the perturbation. >0, upper bound of the rate of change of the disturbance >0, making , If true, then the convergence range of the estimated value error is determined by the sampling time. and filter coefficients The decision is to approach a stable value. ,in, The estimation error of the unknown state estimator. Time corresponding arm end uncertain disturbance , This corresponds to the uncertain disturbance at the joint drive motor end. , For uncertain disturbances, For the derivative of the uncertain disturbance.

[0037] Preferably, the priority matrix Specifically:

[0038] ;

[0039] in, It is a dimension of A vector in which all elements are 1. , , … These are the priority coefficients for obstacle avoidance constraints, saturation constraints, and other constraints, respectively; take... =1, and make satisfy:

[0040] ;

[0041] ;

[0042] ;

[0043] in, The obstacle avoidance constraint weight coefficient; Here are the saturation constraint weight coefficients, where , It is a small constant that is greater than zero;

[0044] to The priorities of each constraint decrease sequentially, while the priorities of saturation constraints and obstacle avoidance constraints are determined by their respective weight coefficients; when a sudden obstacle approaches, To achieve obstacle avoidance, the priority of obstacle avoidance constraints is temporarily increased, motor saturation constraints are relaxed, and joint response is accelerated. When the motor is nearing saturation... Prioritize relaxing performance requirements for accuracy and speed.

[0045] The predictive control method for flexible joint models that considers uncertainties and multiple constraints provided by this invention has the following beneficial effects:

[0046] This invention introduces an unknown state estimator and a constraint-adaptive hierarchical programming method, enabling a flexible joint drive system to achieve joint stability control under the combined effects of uncertain disturbances and multiple constraints. It achieves obstacle avoidance and trajectory tracking of the flexible joint while satisfying as many constraints as possible. The designed unknown state estimator is adapted to the discretized prediction model of MPC, enabling the estimation of uncertain disturbances in the joint even when only the nominal values ​​of model stiffness and inertia are known. This reduces the model dependence of the estimator and its compatibility with the discretized prediction model reduces reliance on the precise model. The constraint-adaptive hierarchical programming method allows the controller to adaptively adjust the priority of constraints such as obstacle avoidance and saturation in response to environmental changes. This improves the controller's autonomous decision-making ability in unstructured environments and solves the problem of unsolvable control laws under conflicting constraints, thus benefiting practical applications. Attached Figure Description

[0047] To more clearly illustrate the embodiments and design schemes of the present invention, the accompanying drawings required for this embodiment will be briefly described below. The drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0048] Figure 1 This is a block diagram illustrating the principle of a flexible joint model predictive control method considering uncertainties and multiple constraints, according to an embodiment of the present invention.

[0049] Figure 2 This is a schematic diagram of a typical flexible joint structure.

[0050] Figure 3 This refers to the positional relationship between two targets considered in the obstacle avoidance constraints of this embodiment of the invention.

[0051] Figure 4 These are simulation results from embodiments of the present invention, considering uncertain disturbances and constraints such as joint obstacle avoidance, saturation, and speed limits. Figure 4 (a) is a simulation result diagram of the joint position; Figure 4 (b) is a simulation result diagram of the joint driving torque; Figure 4 (c) is a simulation result diagram of joint control step length; Figure 4 (d) is the simulation result diagram of the joint angular velocity; Figure 4 (e) is a simulation result diagram of the minimum distance between the joint end and the obstacle.

[0052] Figure 5 This represents the estimation result of the lumped disturbance by the unknown state estimator in this embodiment of the invention. Figure 5 (a) represents the lumped disturbance. The estimation results; Figure 5(b) is for lumped disturbance The estimation results. Detailed Implementation

[0053] To enable those skilled in the art to better understand and implement the technical solutions of the present invention, the present invention will be described in detail below with reference to the accompanying drawings and specific embodiments. The following embodiments are only used to more clearly illustrate the technical solutions of the present invention and should not be construed as limiting the scope of protection of the present invention.

[0054] Example

[0055] The principle block diagram of flexible joint model prediction is as follows: Figure 1 As shown, the position and velocity signals output by the joint drive motor and the articulated arm are collected and fed back to the unknown state estimator. The estimator estimates the lumped disturbance based on the joint motion signals. The estimation results are then output to the prediction model. The model prediction controller uses the current motion state of the joint as feedback information, performs rolling optimization of the control quantity u according to the objective function and the current constraint priority, and finally outputs the optimal control quantity as the input signal of the drive motor to control the joint motion.

[0056] This invention provides a predictive control method for flexible joint models that considers uncertainties and multiple constraints, such as... Figure 1 As shown, the specific steps include:

[0057] Step 1: Establish a joint dynamics model and define lumped disturbances and constraints.

[0058] like Figure 2 The diagram shows a typical physical structure of a flexible joint. A dynamic model of the flexible joint is established using the Lagrange equation method.

[0059] (1)

[0060] in, This refers to the output angle of the reducer; The angle output by the joint; Moment of inertia of the motor rotor and output shaft; The moment of inertia of the joint, including the reducer and the elastic mechanism; This refers to the joint stiffness coefficient; and These are the length and mass of the arm linkage, respectively; This refers to the joint driving torque; and These are the frictional torques on the arm linkage side and the motor side, respectively. and It is an unknown external disturbance.

[0061] To simplify the model complexity, we assume the link moves in a plane perpendicular to the joint output axis, neglecting the link's extension and elastic deformation. We define a lumped disturbance. The nonlinear variations in load mass, friction torque, and joint stiffness are difficult to model precisely; we consider the sum of these nonlinear terms and the external disturbance as the lumped disturbance.

[0062] (2)

[0063] in, It is the increase in joint inertia. Joint stiffness increment, Increment of rotational inertia of the motor rotor and output shaft.

[0064] Assumption Its derivative is bounded, that is, it exists. >0、 >0, making , Established.

[0065] Define state variables Control variables Lumped disturbance variables Output variables This yields a continuous-time model of the flexible joint in its state space:

[0066] (3)

[0067] in , , , , .

[0068] The continuous-time model of the flexible joint state space is discretized using the Euler method to obtain the discrete-time state-space incremental model:

[0069] (4)

[0070] in, , , , , , Sampling time, The sampling period. For state increment, To control the input increment, This represents the lumped disturbance increment.

[0071] Establish constraints. To ensure the joint drive motor has anti-saturation capability and the joint system has the fastest possible response speed and high compliance, consider the following constraints on the system control torque input, control torque increment, arm output angle, and arm output speed:

[0072] (5)

[0073] Among them, for The constraints can prevent the joint drive motor from falling into saturation; the control torque increment The constraints can mitigate the impact response of the drive motor to changes in external disturbances, reducing energy consumption; and The lower bound constraint can improve the joint's motion efficiency while satisfying the saturation constraint; while for and The upper bound constraint satisfies the joint motion space limitation and improves the compliance of the joint response.

[0074] In addition, obstacle avoidance at the end of the arm must be considered during joint movement, such as... Figure 3 As shown.

[0075] Assuming the target and obstacles are strictly convex objects, and It is the point on two objects that is closest to each other. Indicates the direction of a vector between two points. Let represent the minimum distance between two points. and The inertial velocities at the two points are respectively and Then the relative velocities between the two points satisfy the inner product relation. Assuming It is the joint angular velocity. Through the Jacobian matrix Mapping to point On, that is Then the relative velocity between the two points can be expressed as:

[0076] (6)

[0077] Introducing relative velocity damping constraints:

[0078] (7)

[0079] in, Indicates the direction of a vector between two points. The minimum safe distance between the target and the obstacle; The distance from the target to the obstacle is the influence distance; when the distance is less than When doing so, obstacle avoidance needs to be considered; at the same time, a critical distance is also introduced. When the distance is less than At that time, the obstacle avoidance constraint is changed from a soft constraint to a hard constraint; is the convergence coefficient.

[0080] Substituting the relative velocity between the two points into the relative velocity damping constraint, we obtain the obstacle avoidance constraint condition:

[0081] (8)

[0082] Step 2: Design an unknown state estimator and prove its estimation performance.

[0083] Define in the time domain , , , , Filtering variables for joint state variables , , , , :

[0084] (9)

[0085] Substituting the filtered variables into the continuous-time model in the state space of the flexible joint leads to the following two lemmas:

[0086] Lemma 1: Manifold For any positive number All are invariant manifolds, and Established.

[0087] Lemma 2: Manifold For any positive number All are invariant manifolds, and Established.

[0088] Proof: For , Differentiation yields:

[0089] (10)

[0090] Take the Lyapunov equation , Differentiating it and combining it with Young's inequality, we get:

[0091] (11)

[0092] Solving equation (11) yields:

[0093] (12)

[0094] Equation (12) Explanation , , , Both are bounded, and , The exponential convergence is determined by the filter coefficients. The residual set defined by the upper bound of the perturbation:

[0095] (13)

[0096] According to equation (13). At 0, we can get , Established, which means , For any bounded initial conditions , All will converge to 0, so , yes The invariant manifold at time 0. Q.E.D.

[0097] Based on the invariant manifolds in Lemmas 1 and 2, design an unknown state estimator for continuous systems:

[0098] (14)

[0099] For the state variables in model (10) , Applying a low-pass filter, we obtain:

[0100] (15)

[0101] in , It is the filter variable for lumped disturbance. According to equations (14) and (15), we have , Furthermore, it is also possible to introduce... , .

[0102] Define estimation error Then the derivative of the error can be expressed as:

[0103] (16)

[0104] According to equation (16), the following theorem can be obtained:

[0105] Theorem 1: For a continuous-time model in the state space of a flexible joint, the estimation error of the unknown state estimator... The exponential convergence to the neighborhood of zero is bounded by the following upper bound:

[0106] (17)

[0107] And when From time to time Established.

[0108] Proof: For example, let's take the Lyapunov equation as... Differentiating it and applying Young's inequality, we can obtain:

[0109] (18)

[0110] Solving equation (18) yields the following results. Established, and further obtained Established, and when From time to time Established.

[0111] Take the Lyapunov equation as ,get Similarly, when From time to time Established. Q.E.D.

[0112] So far, it has been proven that the estimate of the unknown state estimator is bounded in continuous-time systems. Now, we will discretize it and prove the boundedness of the estimator in discrete systems.

[0113] Discretizing the filter variables in equation (9) using the Euler method yields:

[0114] (19)

[0115] Substituting the filter variables into equation (14), we obtain the discretized unknown state estimator:

[0116] (20)

[0117] For estimation error Using the same discretization process, we obtain:

[0118] (twenty one)

[0119] Since the uncertain disturbance and its derivative are bounded, the estimation error satisfies the linear difference inequality:

[0120] (twenty two)

[0121] because Then the solution of the difference equation approaches a stable value:

[0122] (twenty three)

[0123] That is, for any bounded uncertain disturbance, the estimation error of the discrete unknown state estimator will eventually converge to a state defined by... and Within the determined range. Thus, the discretized unknown state estimator, knowing only the joint inertia, motor rotational inertia, and nominal values ​​of stiffness parameters, achieves the estimation of discrete lumped disturbances, and the estimation error is bounded.

[0124] Step 3: Build a predictive model and design the optimal control problem.

[0125] Let the prediction time domain of MPC be... Control time domain is ,and ≤ We make the following assumption: outside the control time domain, the control quantity remains unchanged, i.e. ; aggregated disturbance exist It remains unchanged after a certain time, that is Let the current time be... The current state measurement value is Define the prediction time domain Inside The predicted output vector is , The predicted output vector is and control time domain Intrapredictive control output increment vector Then the system in the future The output prediction equation for step 1 is:

[0126] (twenty four)

[0127] in, , , , , Form and Same, use replace Matrix in You can get This will not be elaborated upon here.

[0128] The desired control objective is to determine the future motion trajectory of the joint. Able to effectively track reference trajectory ,in From the prediction time domain The reference input sequence is formed by the given input signals; at the same time, the changes in control actions should be avoided as much as possible to reduce the risk of drive motor saturation and reduce energy consumption during changes in motion state. Therefore, the objective function of the design optimization problem is:

[0129] (25)

[0130] in, The weighted coefficient matrix for predicting output error. The larger the value, the closer the desired control output is to the reference input; To control the incremental weighting coefficient matrix, The larger the value, the smaller the expected control action at that moment.

[0131] Based on the objective function, and combined with the discrete-time state-space incremental model and constraints (5) and (8), the multi-constraint flexible joint anti-saturation optimization control problem based on MPC can be described as follows:

[0132] Question 1: Finding the optimal predictive control increment :

[0133] (26)

[0134] Satisfy the dynamic equations,

[0135] ,

[0136] and constraints

[0137] .

[0138] Step 4: Constraint transformation, refactoring Problem 1 into a quadratic programming problem.

[0139] Given the presence of multiple complex constraints, it is difficult to find a specific analytical solution for Problem 1. However, since the objective function of the optimization problem is quadratic and the constraints are linear, Problem 1 can be viewed as a quadratic programming (QP) problem. Therefore, numerical optimization methods can be used to obtain a numerical solution for Problem 1 for system control.

[0140] The reconstructed objective function can be written as:

[0141] (27)

[0142] in, It is a Hessian matrix; The gradient vector matrix, .

[0143] In Question 1, the optimization variables The constraints can be written as:

[0144] (28)

[0145] because Therefore, for any All Furthermore, regarding question 1... The constraints can be rewritten as:

[0146] (29)

[0147] Regarding question 1 and The constraints are denoted as follows:

[0148] (30)

[0149] Then you can talk about and Rewrite the constraints as about Inequality form:

[0150] (31)

[0151] For the obstacle avoidance constraint in Problem 1, the prediction equation for the joint angular velocity is as shown in equation (24). As shown, therefore:

[0152] (32)

[0153] After sorting, we can obtain:

[0154] (33)

[0155] in, , , .

[0156] According to equations (27), (28), (29), (31), and (33), problem 1 can be transformed into a quadratic programming problem:

[0157] Question 2: Finding the optimal predictive control increment :

[0158] (34)

[0159] in, , .

[0160] Thus, the problem of optimizing and controlling flexible joints with complex constraints such as torque, velocity, and obstacle avoidance is transformed into a quadratic programming problem with inequality constraints, thereby improving the solvability of the problem.

[0161] Step 5: Introduce the constrained adaptive hierarchical programming method to reconstruct the objective function of Problem 2.

[0162] In Problem 2, the simultaneous presence of complex constraints such as joint torques, velocities, and obstacle avoidance, as well as uncertain disturbances such as unmodeled dynamics and unknown disturbances, often leads to suboptimal solutions or even no solutions, affecting its practical application. This paper proposes a multi-constraint hierarchical programming method to ensure a solution to the control problem is obtained while maximizing constraint satisfaction.

[0163] Introducing the slack variable ε, equation (34) is reconstructed as follows:

[0164] (35)

[0165] in, For slack variables The matrix formed; This is the slack variable weight matrix; The matrix that forms the upper bounds of each relaxation variable; It is a matrix in which all elements except the diagonal elements are 0, and the diagonal elements are either 0 or 1, indicating that the corresponding constraints are hard or soft constraints. When problem 2 is unsolvable, some slack variables will be taken as positive values ​​to relax the constraints, thus making the problem solvable.

[0166] We employ hierarchical programming to ensure that Problem 2 is solvable while maximizing the slack variables of each constraint to zero. We divide all constraints into... Level, of which the first Level includes There are several constraints, where j = 1, 2, ... And Level 1 to Level 2 The priority of constraints decreases progressively. When a problem becomes unsolvable, the slack variables corresponding to lower priority constraints will be the first to be non-zero.

[0167] For each slack variable Define logical variables All logical variables together form a logical variable matrix. When the constraints are not satisfied, there is At this point, the logical variable is taken. .

[0168] Based on equation (35), a logical variable matrix is ​​further introduced. and priority matrix Problem 2 can be restructured into a QP problem of constrained adaptive hierarchical programming:

[0169] Question 3: Finding the optimal predictive control increment ,

[0170] (36)

[0171] Among them, matrix Defined as:

[0172] (37)

[0173] in, It is a dimension of A vector in which all elements are 1. , , … These are the priority coefficients for obstacle avoidance constraints, saturation constraints, and other constraints, respectively; take... =1, and make satisfy:

[0174] (38)

[0175] in, This refers to the obstacle avoidance weighting coefficient. Here, is the saturation weighting coefficient, where , It is a small constant greater than zero, and can be taken as 0.01.

[0176] to The priorities of each constraint decrease sequentially, while the priorities of saturation constraints and obstacle avoidance constraints are determined by their respective weight coefficients. When a sudden obstacle approaches, Temporarily increase the priority of obstacle avoidance constraints, relax motor saturation constraints, and enable joints to respond quickly to achieve obstacle avoidance; when the motor is close to saturation... This prioritizes relaxing performance requirements such as accuracy and speed to ensure system stability. Thus, it achieves adaptive adjustment of constraint priorities, resolving the issue that static hierarchical programming may lead to suboptimal solutions or the ineffectiveness of obstacle avoidance and saturation constraints.

[0177] Thus, the design of the model predictive control method for flexible joints considering uncertain disturbances and multiple constraint conflicts is complete. The optimal control problem for flexible joints, including uncertain disturbances and inequality constraints, is given in Problem 3. The lumped disturbance, consisting of unmodeled dynamics of the flexible joint and unknown disturbances, is estimated by a discrete unknown state estimator. The constraint priority allocation will be autonomously adjusted by adaptive weights based on the joint motion state and used to solve the control law. Solving the objective function shown in Problem 3 yields the optimal control law for the flexible joint simultaneously considering uncertain disturbances and multiple constraints. The control is thus completed.

[0178] To verify the effectiveness of this invention, the proposed method was simulated. =0.45 kg•m²; =0.062 kg•m²; =15 N•m / rad; =5; Filter coefficient =0.1. During the simulation, the unmodeled dynamics and unknown disturbances are taken as follows:

[0179]

[0180] The initial joint angle is set to 0 rad, and the given trajectory is sin(2t). During the simulation, the angle restriction zone is set to 0.9–1.1 rad from 0.8 to 1.2 s, and to 0–0.1 rad from 3.0 to 3.2 s. The angle restriction zone is used to simulate obstacles during joint movement, and a safe distance from these obstacles is set. =0.05rad, affecting distance =0.2rad, critical distance =0.1rad, obstacle avoidance convergence coefficient Set the joint angle and angular velocity to no more than 1.5 rad and 3 rad / s, respectively. Set the saturation constraint for the joint driving torque to ±10 N•m. Set the controller control increment ∆U ≤ 2. Initially, set all constraints to have the same priority, and set all weight coefficients in matrix S to 1. Set other controller parameters as follows: =10, =3, , These are the identity matrices for their respective dimensions. The simulation duration is 5 seconds. The simulation results are as follows: Figure 4 , 5 As shown.

[0181] Figure 4(a) shows the joint position curves for the joint tracking a given trajectory. First, it can be seen that the joint can smoothly adjust its trajectory when approaching an obstacle to avoid entering the angular restricted area within the specified time, indicating that the constraint mechanism is effective in the proposed control strategy. Second, it can also be seen that outside the obstacle's influence distance, the joint can track the given trajectory well; however, when the joint moves into the obstacle's influence distance, obstacle avoidance constraints are triggered. At this time, the controller adjusts the priority of the obstacle avoidance constraints to the highest level, and the joint response ultimately sacrifices tracking performance to achieve obstacle avoidance, and can re-track the given trajectory after obstacle avoidance. This shows that the adaptive adjustment strategy of constraint priority in this invention is effective. The controller can adaptively adjust the priority between multiple constraints according to the current motion state of the joint, solving the problem that the objective function may be unsolvable under multiple constraints.

[0182] Figure 4 Figure (b) shows the variation process of the joint driving torque. The dashed line represents the saturation constraint of the joint driving torque, the solid line represents the actual variation curve of the joint driving torque, and the shaded area represents the time interval where obstacles are present. First, it can be seen that the maximum driving torque during joint movement is approximately 7.25 N•m, with a saturation of approximately 72.5%, indicating that the joint torque saturation constraint is well satisfied. Second, when the joint approaches an obstacle, the adaptive constraint mechanism ensures that the priority of obstacle avoidance constraints is increased, allowing for a significant adjustment of the joint torque when approaching an obstacle, enabling rapid obstacle avoidance.

[0183] Figure 4 Figure (c) shows the variation curve of the joint control step size ∆U. It can be seen that as the joint approaches the obstacle, ∆U undergoes a significant change to provide a larger control torque for obstacle avoidance, but the amplitude of the control step size does not exceed the constraint limit, demonstrating the effectiveness of the constraint. Furthermore, the maximum saturation of ∆U is greater than the saturation of the torque constraint, indicating that the constraint priority of ∆U is lower than that of the torque constraint, proving the effectiveness of the proposed method in adaptively adjusting constraint priority.

[0184] Figure 4 Figure (d) shows the curve of angular velocity change during joint movement. It can be seen that there is no violation of constraints during the joint angular velocity obstacle avoidance process, and the given velocity curve can be tracked well after the obstacle avoidance is completed.

[0185] Figure 4 (e) represents the minimum distance between the joint distal end and the obstacle within the obstacle's range. It can be seen that the minimum distance between the joint and the obstacle is always greater than the set safety distance. This indicates that the control method proposed in this paper can ensure that the flexible articulated arm can effectively avoid obstacles during movement and ensure that the joint is always kept at a safe distance.

[0186] Figure 5 This is the estimation result of the lumped disturbance by the unknown state estimator. Because the disturbance includes unmodeled dynamic terms related to joint position and velocity, the lumped disturbance curve exhibits a noticeable oscillating convergence process during joint obstacle avoidance. However, this does not affect the estimator's tracking performance; the estimator output can still effectively track the lumped disturbance.

[0187] It should be noted that the specific embodiments described above enable those skilled in the art to more fully understand the present invention, but do not limit the present invention in any way. Therefore, although the present invention has been described in detail in this specification and embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the present invention; and all technical solutions and improvements that do not depart from the spirit and scope of the present invention are covered within the protection scope of the present invention patent. No reference numerals in the claims should be construed as limiting the scope of the claims. Any simple variations or equivalent substitutions of technical solutions that can be readily obtained by those skilled in the art within the scope of the technology disclosed in the present invention are within the protection scope of the present invention.

Claims

1. A flexible joint model predictive control method considering uncertainty and multiple constraints, characterized in that, Includes the following steps: A flexible joint dynamic model including lumped perturbation is constructed and multiple constraints are set. Based on the flexible joint dynamic model, joint motion state data and model nominal values ​​are obtained. The joint motion state and the model nominal value are input into the unknown state estimator to obtain the estimated value of the lumped disturbance; the lumped disturbance specifically includes the joint inertia increment, joint stiffness increment, motor rotor and output shaft rotational inertia increment, frictional torque between the arm link side and the motor side, nonlinear gravity terms of the joint and arm link, and external unknown disturbances. A prediction model is constructed by inputting the joint motion state data and the estimated value of the lumped disturbance into the prediction model to construct a flexible joint multi-constraint optimization control problem. The flexible joint multi-constraint optimization control problem is transformed into a quadratic programming problem, relaxation variables are introduced, and adaptive hierarchical programming is performed on the multi-constraint conditions based on a priority matrix dynamically generated by obstacle avoidance weight coefficients and saturation weight coefficients to obtain a reconstructed objective function. The optimal predictive control increment is obtained by solving the reconstructed objective function, and the optimal predictive control increment is used as the input signal of the drive motor to control the joint motion. The obstacle avoidance weight coefficient is dynamically adjusted based on the real-time distance between the joint and the obstacle, and the saturation weight coefficient is dynamically adjusted based on the real-time margin of the joint driving torque.

2. The method of model predictive control of a flexible joint according to claim 1, wherein, The specific dynamic model of the flexible joint is as follows: ； where, is the output angle of the reducer; is the output angle of the joint; is the moment of inertia of the motor rotor and the output shaft; is the inertia of the joint, including the reducer and the elastic mechanism; is the joint stiffness coefficient; and are the length and mass of the arm link, respectively, is the acceleration of gravity; is the joint driving torque; and are the friction torques on the arm link side and the motor side, respectively; and is the external unknown disturbance; The sum of the nonlinear terms and external disturbances in the dynamic model of the flexible joint is considered as a lumped disturbance, which is specifically: ； wherein, is the arm end lumped disturbance, is the joint drive motor end lumped disturbance, is the joint inertia increment, is the joint stiffness increment, is the motor rotor and output shaft rotational inertia increment, is the reducer angular acceleration, is the joint angular acceleration.

3. The method of model predictive control of a flexible joint with uncertainty and multiple constraints according to claim 1, wherein, The multiple constraints specifically include the driving torque constraint of the joint drive motor, the control increment constraint of the joint drive motor, the joint angle constraint, the joint speed constraint, and the obstacle avoidance constraint. The specific driving torque constraint of the joint drive motor is as follows: ； The incremental constraint for the joint drive motor control is specifically as follows: ； The joint angle constraint is specifically as follows: ； The joint velocity constraint is specifically as follows: ； wherein, a driving torque input control amount for the joint driving motor, a control increment, a joint angle, a joint speed; The obstacle avoidance constraints are specifically as follows: ； wherein is the inertial velocity at the point; is the joint angular velocity; is the Jacobian matrix the transpose of is the minimum distance between two points; denotes the vector direction between two points, is the minimum safety distance of the target to the obstacle; is the influence distance of the target to the obstacle.

4. The method of model predictive control of a flexible joint with uncertainty and multiple constraints according to claim 1, wherein, The reconstructed objective function is specifically as follows: ； ； in, , ; Let be the state variable matrix of the joint system, where These are the rotation angle and velocity output by the joint, respectively. These are the rotation angle and speed output by the reducer, respectively; It is the identity matrix; , , , , , These are all coefficient matrices composed of joint dynamic parameters, which are related to the system model; , are the output matrices composed of the joint angles and velocities of each cycle; Input quantity for joint control; This represents the lumped disturbance matrix of the joint drive system, which consists of uncertain disturbances such as unmodeled dynamics and unknown disturbances. For the lumped disturbance at the arm end, The lumped disturbance at the joint drive motor end; These are the maximum and minimum values ​​of the corresponding variables, used to describe the constraint requirements; , , The coefficient matrices of the obstacle avoidance constraints, the minimum safe distance involved in obstacle avoidance, and the minimum safe distance involved in obstacle avoidance are used to describe the obstacle avoidance constraint requirements; To control the increment; For slack variables The constructed matrix divides all constraints into Level, of which the first Level includes There are several constraints, where j = 1, 2, ... ; This is the slack variable weight matrix; The matrix that forms the upper bounds of each relaxation variable; It is a matrix in which all elements except the diagonal elements are 0. The diagonal elements are either 0 or 1, which indicates that the corresponding constraint is a hard constraint or a soft constraint. Let be a matrix of logical variables, where each logical variable satisfies ; This is the priority matrix; To constrain the convergence coefficients, satisfying ; This is the transpose of the slack variable; It is a Hessian matrix. Gradient vector matrix The transpose of .

5. The predictive control method for flexible joint models considering uncertainties and multiple constraints according to claim 1, characterized in that, The specific details of the unknown state estimator are as follows: ； in, This represents the nominal value of joint stiffness. This is the nominal value of the motor's end inertia. This represents the nominal value of the moment of inertia of the articulated arm. These are the filter coefficients of the low-pass filter; , , , , State variables , , , as well as The filter variable, For the rotation angle on the side of the joint drive motor, For the speed of the joint drive motor side, For the angle of the joint on the side of the arm, Rotation speed of the joint on the arm side, For driving torque; The sampling period; Sampling time; , This represents the lumped disturbance estimate obtained for each sampling period.

6. The predictive control method for flexible joint models considering uncertainties and multiple constraints according to claim 5, characterized in that, The error of the estimated value calculated by the unknown state estimator is bounded, and we assume that there exists an upper bound on the perturbation. >0, upper bound of the rate of change of the disturbance >0, making , If true, then the convergence range of the estimated value error is determined by the sampling time. and filter coefficients The decision is to approach a stable value. ,in, The estimation error of the unknown state estimator. Time corresponding arm end uncertain disturbance , This corresponds to the uncertain disturbance at the joint drive motor end. , For uncertain disturbances, The derivative is the unknown perturbation derivative.

7. The predictive control method for flexible joint models considering uncertainties and multiple constraints according to claim 4, characterized in that, The priority matrix Specifically: ； in, It is a dimension of A vector in which all elements are 1. , , … These are the priority coefficients for obstacle avoidance constraints, saturation constraints, and other constraints, respectively; take... =1, and make satisfy: ； ； ； in, The obstacle avoidance constraint weight coefficient; Here are the saturation constraint weight coefficients, where , It is a small constant that is greater than zero; to The priorities of each constraint decrease sequentially, while the priorities of saturation constraints and obstacle avoidance constraints are determined by their respective weight coefficients; when a sudden obstacle approaches, To achieve obstacle avoidance, the priority of obstacle avoidance constraints is temporarily increased, motor saturation constraints are relaxed, and joint response is improved. When the motor is nearing saturation, Prioritize relaxing performance requirements for accuracy and speed.

Images