Wheeled mobile robot trajectory tracking robust control method based on feasible set

Abstract

The invention discloses a wheeled mobile robot trajectory tracking robust control method based on a feasible set, and belongs to the technical field of wheeled mobile robot trajectory tracking. The method comprises the following steps: establishing a track error system of the wheeled mobile robot; based on Lipschitz characteristics of a disturbance change rate and a set membership estimation method, constructing a dynamic feasible set representing a disturbed state range of the system; and adding the feasible set as a key constraint into a model predictive control (MPC) framework, and designing a feasible set iterative contraction mechanism. According to the invention, high-precision and high-robustness trajectory tracking can be realized under the condition of complex time-varying external disturbance.

Description

Technical Field

[0001] This invention belongs to the field of trajectory tracking technology for wheeled mobile robots, specifically relating to a robust control method for trajectory tracking of wheeled mobile robots based on feasible sets. Background Technology

[0002] Wheeled mobile robots are intelligent devices with autonomous navigation capabilities. They can perceive their environment through sensors and combine this with control algorithms to achieve trajectory tracking in various environments. Trajectory tracking is a dynamic control process in which the wheeled mobile robot adjusts its posture in real time based on control algorithms to ensure that its actual motion trajectory matches the preset path. In this process, the robot needs to overcome a series of problems such as disturbances and constraints. Existing mainstream control methods, such as Model Predictive Control (MPC), which handles state and control constraints based on a rolling optimization framework, can theoretically guarantee feasibility. However, its robustness is highly dependent on an accurate model and is prone to instability under unknown external disturbances. Robust Model Predictive Control (RMPC), although it can explicitly compensate for model errors and disturbances, perform online rolling optimization, and satisfy constraints, relies too heavily on the precise description of the uncertainty set.

[0003] Among common trajectory tracking RMPC methods, there is a type of tubular MPC: Traditional tubular MPC can constrain the trajectory deviation affected by disturbances within a preset area by designing a "robust invariant set". However, this invariant set usually needs to be calculated offline, which makes it difficult to handle dynamic time-varying disturbances. Moreover, due to geometric conservatism, a trade-off must be made between robustness and dynamic performance optimization. Adaptive tubular MPC combines online parameter estimation, which can adjust the size and shape of the robust invariant set in real time. However, this parameter estimation has high computational complexity and may also compromise the robustness of the controlled system.

[0004] The trajectory tracking process of wheeled mobile robots is a dynamic process, and the limitations of the methods mentioned above can significantly affect system performance. Relying on invariant sets calculated offline not only fails to adapt to time-varying disturbances but also requires consideration of the trade-off between robustness and dynamic performance, leading to decreased trajectory tracking accuracy. Furthermore, the high computational complexity severely impacts tracking reliability in dynamic scenarios. Therefore, designing a robust control method that is solvable in real-time and balances robustness and dynamic performance is of great significance. Summary of the Invention

[0005] To address the limitations of existing robust control methods for trajectory tracking, this invention proposes a robust control method for wheeled mobile robots based on feasible sets. Using the Lipschitz continuity of the rate of change of disturbance as core prior knowledge, it constructs disturbance increment constraints and then uses set membership estimation to construct feasible sets. Leveraging the advantage of MPC in effectively handling constraints, a control input constraint based on feasible sets is considered. Iterative shrinkage of the feasible set effectively suppresses the impact of disturbances on the trajectory tracking of the wheeled mobile robot, thereby achieving accurate trajectory tracking under complex time-varying disturbance conditions.

[0006] To achieve the above objectives, the present invention adopts the following solution:

[0007] A robust control method for trajectory tracking of a wheeled mobile robot based on feasible sets, characterized by the following steps:

[0008] Step 1: Establish the trajectory error system of the wheeled mobile robot. Specifically, this includes establishing an actual error system based on the deviation between the actual trajectory affected by disturbance and the set desired trajectory. Its state space description is denoted as the actual form of the error system. It also includes establishing a nominal error system based on the deviation between the nominal trajectory ignoring the influence of disturbance and the set desired trajectory. Its state space description is denoted as the nominal form of the error system.

[0009] Step 2: Based on the actual form of the error system, construct the feasible set of perturbations in the system by combining the dynamic characteristics of the perturbation with the geometric evolution of the feasible set;

[0010] Step 3: Parameterize the control strategy and design control input equality constraints based on feasible sets, using affine disturbance feedback as the basic form.

[0011] Step 4: Based on the nominal form of the error system and considering the constraints in Steps 2 and 3, design the RMPC algorithm to achieve robust performance of the system.

[0012] Furthermore, in step 1, the desired trajectory and corresponding pose are considered as a virtual leader, and the actual trajectory is considered as a follower. The robot's "head position" is used as the trajectory coordinates during the movement process, and its kinematic model is expressed as follows:

[0013]

[0014] Where, the state variable ξ = [x yθ] Τ x and y represent the position information of the wheeled mobile robot along the x and y axes, respectively, and θ is the steering angle; the control input u = [vω] Τ v is the linear velocity, ω is the angular velocity; v and ω satisfy a rhombus constraint u∈U, This constraint can effectively prevent wheeled mobile robots from failing to operate normally due to a certain control variable being 0 during operation;

[0015] Establish the Frenet-Serret coordinate system with the leader and followers as the organism centers. r O and f In the follower coordinate system, the coordinate deviation between the leader and the follower is the positional deviation between them. Transforming the leader's coordinates to the follower coordinate system yields the positional deviation:

[0016]

[0017] Where, p e =[x e y e ] Τ This represents the error between the follower and the leader, where h represents the distance from the center of the organism to the head, and x represents the error between the follower and the leader. r y r θ r Indicates the leader's pose information, x f y f θ f Indicates the pose information of the follower;

[0018] Differentiate formula (2):

[0019]

[0020] Where, θ e =θ r -θ f v r v f ω represents the linear velocity of the leader and the follower, respectively. f Indicates the angular velocity of the follower;

[0021] make B = I, Formula (3) can be simplified as follows:

[0022]

[0023] Formula (4) is the dynamic model of tracking error. Based on this, the trajectory error system of the wheeled mobile robot is established, which includes the actual error system based on the deviation between the actual trajectory affected by disturbance and the expected trajectory, and the nominal error system based on the deviation between the nominal trajectory and the expected trajectory ignoring the influence of disturbance.

[0024] The nominal form of positional deviation is:

[0025]

[0026] in, A nominal form representing the positional deviation between followers and leaders. The nominal value for the follower's pose information;

[0027] Taking the derivative of Equation 5, we obtain the nominal form of the error system:

[0028]

[0029] in, x e y e θ e v f ω f The nominal form;

[0030] Formula (6) can be simplified as:

[0031]

[0032] Taking the disturbance d into account in the deviation, formula (2) becomes:

[0033]

[0034] Where, d x d y These represent disturbances in the x and y directions of the tracking trajectory, respectively.

[0035] Taking the derivative of equation (8), we obtain the actual form of the error system as follows:

[0036]

[0037] make Equation (9) can be simplified to:

[0038]

[0039] Define {t k :k∈N,t k+1 -t k =δ}, δ>0 represents the sampling time series. An optimization problem is solved once at each sampling time, giving the cost function J that needs to be optimized in model predictive control. For the sake of model accuracy, this cost function J consists of nominal quantities:

[0040]

[0041] in, Indicates stage cost, Let T represent the terminal cost, Q, P, and R be positive definite matrices, and T be the prediction time domain, satisfying T = Nδ, N ∈ N, where δ represents the fixed step size.

[0042] Furthermore, step 2 specifically includes:

[0043] Assume that the disturbance in the actual form of the error system satisfies the condition of being unknown but bounded:

[0044]

[0045] in, Indicates the boundary of the disturbance;

[0046] Furthermore, the dynamic characteristics of the perturbation d satisfy the Lipschitz continuity, i.e., there exists a Lipschitz constant L. d A value greater than 0 makes the rate of change of the disturbance globally bounded. This assumption transforms the discrete-time perturbation increment constraint into:

[0047]

[0048] Where P = -L d δ, Indicates the upper and lower bounds of the rate of change of the disturbance;

[0049] Incorporating this disturbance into the actual form of the error system, i.e., formula (10), and using the forward Euler method, discretize formula (10) by letting:

[0050]

[0051] Then, formula (10) becomes:

[0052]

[0053] The disturbance change Δd(t) can be extracted from formula (14). k )=d(t k )-d(t k-1 ), further obtaining Δd(t) k The expression for ) is:

[0054] Δd(t k ) = R -1 (θ f (t k-1 ))(p e (t k )-A k p e (t k-1 )-B k u e (t k-1 (16)

[0055] Comparing formulas (5) and (8), it can be seen that the disturbance d can be separated in formula (8), that is, the two formulas satisfy the following approximate relationship:

[0056]

[0057] Substituting formula (17) into formula (16), we get:

[0058]

[0059] in,

[0060] Based on the assumptions mentioned in formula (13): Thus, the perturbation at t is obtained. k Feasible domain at time

[0061] For any time t q , t q ≤t k The corresponding feasible region for:

[0062]

[0063] Finally, the feasible set Θ is obtained. k The expression:

[0064]

[0065] Where Ω0 is a custom initial feasible set.

[0066] Furthermore, step 3 specifically includes:

[0067] For all k∈{t k ,t k +1,...,t k Within the MPC prediction range of length N, the control strategy parameterization is as follows: +N-1}

[0068]

[0069] Wherein, M(k|t k ) is time t k The planned feedback gain, v(k|t) k Both are auxiliary inputs and decision variables in the optimization problem; the perturbations stacked along each prediction interval of the MPC are... Defined as:

[0070]

[0071] Then, the predicted input sequences in formula (22) are stacked together to obtain the parameterized control strategy:

[0072]

[0073] For any time t k Matrix M(t) k ) and v(t) k All of these were obtained by arranging the decision variables as follows:

[0074]

[0075] Furthermore, step 4 specifically includes:

[0076] Using the forward Euler method and a fixed step size δ, the nominal error system (7) and the actual error system (10) are discretized:

[0077]

[0078] p e (t k ) = A k ·p e (t k-1 )+B k ·u e (t k-1 )+R(θ f (t k-1 ))·(d(t k )-d(t k-1 (27)

[0079] in,

[0080] Let the prediction time domain of MPC be N, p e (k|t k ) indicates at time t k Given any possible perturbation, the predicted state quantity for the k-th prediction interval is obtained by predicting the control input sequence {u}. e (t k |t k ),u e (t k +1|t k ),...,u e (t k +N-1|t k The result is obtained when k = t in the actual error system (10). k +N uses terminal control input u Ω ;

[0081] After obtaining the predicted values ​​of each state variable, t is iteratively calculated based on the feasible set of perturbations constructed in step 2. k The prediction feasible sets {Θ} corresponding to the N prediction time domains at time points k (t k |t k ),Θ k (t k +1|t k ),......,Θ k (t k +N|t k These feasible sets of predictions are integrated into the prediction model in the form of constraints, forming a constrained nominal prediction model;

[0082] The predicted value of the control quantity is then used to constrain the control input in the form of an equation based on the control strategy obtained in step 3. At the same time, the planning feedback gain M is added to the decision variable by means of parameterization, and the complex robust constraint handling problem is transformed into a convex optimization problem.

[0083] Based on the discrete nominal error system, the cost function J that needs to be optimized in discrete-time model predictive control is given. * :

[0084]

[0085] in, These are the stage cost and the terminal cost, which are composed of discrete variables, respectively.

[0086] At each step, minimize the following optimization problem:

[0087]

[0088] Where Ω represents the terminal set of tracking errors, which is an invariant set. The perturbation estimate corresponding to the k-th prediction time domain.

[0089] The beneficial technical effects of this invention are as follows:

[0090] 1) This invention designs a robust control method for trajectory tracking of a wheeled mobile robot based on feasible sets. By deeply coupling the disturbance change rate constraint with the state space model of the error system, the robustness of trajectory tracking is enhanced in two ways. 2) The feasible set in this invention can be updated online and iteratively shrunk, which increases the real-time performance of the control, enables the trajectory error to converge quickly, and effectively suppresses the influence of disturbances. 3) This invention takes the error system composed of trajectory errors as the core control object, combines MPC rolling optimization and feasible set constraints, and transforms the disturbance uncertainty into an online optimization problem with geometric constraints. Attached Figure Description

[0091] Figure 1 This is a flowchart of the control method in this invention;

[0092] Figure 2 This is a leader-follower structure diagram in this invention;

[0093] Figure 3 This is a simulation curve of trajectory tracking in this invention; Detailed Implementation

[0094] The specific embodiments of the present invention will be further described below with reference to specific examples:

[0095] A robust control method for trajectory tracking of a wheeled mobile robot based on feasible sets, such as Figure 1 As shown, it includes the following steps:

[0096] Step 1: Establish the trajectory error system of the wheeled mobile robot. Specifically, this includes establishing an actual error system based on the deviation between the actual trajectory affected by disturbance and the set desired trajectory. Its state space description is denoted as the actual form of the error system. It also includes establishing a nominal error system based on the deviation between the nominal trajectory ignoring the influence of disturbance and the set desired trajectory. Its state space description is denoted as the nominal form of the error system.

[0097] Specifically, the desired trajectory and its corresponding pose are considered as a virtual leader, and the actual trajectory is considered as a follower. The error system of this invention is implemented through a leader-follower method, with the specific structure as follows: Figure 2 As shown. Considering the non-holonomic constraints of the wheeled mobile robot, and taking the robot's "head position" as the trajectory coordinates during motion, its kinematic model is expressed as:

[0098]

[0099] Where, the state variable ξ = [x yθ] Τ x and y represent the position information of the wheeled mobile robot along the x and y axes, respectively, and θ is the steering angle; the control input u = [vω] Τ v is the linear velocity, ω is the angular velocity; v and ω satisfy a rhombus constraint u∈U, This constraint can effectively prevent wheeled mobile robots from failing to operate normally due to a certain control variable being 0 during operation;

[0100] Establish the Frenet-Serret coordinate system with the leader and followers as the organism centers. r O and f In the follower coordinate system, the coordinate deviation between the leader and the follower is the positional deviation between them. Transforming the leader's coordinates to the follower coordinate system yields the positional deviation:

[0101]

[0102] Where, p e =[x e y e ] Τ This represents the error between the follower and the leader, where h represents the distance from the center of the organism to the head, and x represents the error between the follower and the leader. r y r θ r Indicates the leader's pose information, x f y f θ f Indicates the pose information of the follower;

[0103] Differentiate formula (2):

[0104]

[0105] Where, θ e =θ r -θ f v r v f ω represents the linear velocity of the leader and the follower, respectively. r ω f Indicates the angular velocity of the leader and followers;

[0106] make B = I, Formula (3) can be simplified as follows:

[0107]

[0108] Formula (4) is the dynamic model of the tracking error. Based on this, a trajectory error system for a wheeled mobile robot is established, specifically including establishing an actual error system based on the deviation between the actual trajectory affected by disturbance and the desired trajectory, the state space of which is denoted as the actual form of the error system; and establishing a nominal error system based on the deviation between the nominal trajectory and the desired trajectory ignoring the influence of disturbance, the state space of which is denoted as the nominal form of the error system. Since model predictive control usually relies on an accurate model, and disturbances are unknown, this patent uses the nominal form to construct the cost function and the actual form to construct the feasible set. For ease of distinction, the superscript ~ is used to represent the nominal quantity, that is:

[0109] The nominal form of positional deviation is:

[0110]

[0111] in, A nominal form representing the positional deviation between followers and leaders. This is the nominal value of the steering angle;

[0112] The nominal form of the error system is:

[0113]

[0114] in, x e y e θ e ω f v f The nominal form;

[0115] make Abbreviated as:

[0116]

[0117] The absence of a superscript indicates the actual quantity with disturbance d. Taking the disturbance d into account in the deviation, formula (2) becomes:

[0118]

[0119] Where, d x d y These represent disturbances in the x and y directions of the tracking trajectory, respectively.

[0120] Taking the derivative of equation (8), we obtain the actual form of the error system as follows:

[0121]

[0122] make B t =I, Equation (9) can be simplified to:

[0123]

[0124] Among them, A t B t Represent the state matrix and control matrix of the actual error system;

[0125] Define {t k :k∈N,t k+1 -t k =δ}, δ>0 represents the sampling time series. An optimization problem is solved once at each sampling time, giving the cost function J that needs to be optimized in model predictive control. For the sake of model accuracy, this cost function J consists of nominal quantities:

[0126]

[0127] in, Indicates stage cost, Let T represent the terminal cost, Q, P, and R be positive definite matrices, and T be the prediction time domain, satisfying T = Nδ, N ∈ N, where δ represents the fixed step size.

[0128] Step 2: Based on the actual form of the error system, construct the feasible set of perturbations in the system by combining the dynamic characteristics of the perturbation with the geometric evolution of the feasible set;

[0129] Specifically, it is assumed that the disturbance in the actual form of the error system satisfies the condition of being unknown but bounded:

[0130]

[0131] in, Indicates the boundary of the disturbance;

[0132] Furthermore, the dynamic characteristics of the perturbation d satisfy the Lipschitz continuity, i.e., there exists a Lipschitz constant L. d A value greater than 0 makes the rate of change of the disturbance globally bounded. This assumption transforms the discrete-time perturbation increment constraint into:

[0133]

[0134] Where P = -L d δ, Indicates the upper and lower bounds of the rate of change of the disturbance;

[0135] Incorporating this disturbance into the actual form of the error system, i.e., formula (10), and using the forward Euler method, discretize formula (10) by letting:

[0136]

[0137] Then, formula (10) becomes:

[0138]

[0139] The disturbance change Δd(t) can be extracted from formula (14). k )=d(t k )-d(t k-1 ), further obtaining Δd(t) k The expression for ) is:

[0140] Δd(t k ) = R -1 (θ f (t k-1 ))(p e (t k )-A k p e (t k-1 )-B ku e (t k-1 (16)

[0141] Comparing formulas (5) and (8), it can be seen that the disturbance d can be separated in formula (8), that is, the two formulas satisfy the following approximate relationship:

[0142]

[0143] Substituting formula (17) into formula (16), we get:

[0144]

[0145] in,

[0146] Based on the assumptions mentioned in formula (13), we get: Thus, the perturbation at t is obtained. k Feasible domain at time

[0147] For any time t q , t q ≤t k The corresponding feasible region for:

[0148]

[0149] Finally, the feasible set Θ is obtained. k The expression:

[0150]

[0151] Where Ω0 is a custom initial feasible set.

[0152] Step 3: Parameterize the control strategy and design control input equality constraints based on feasible sets, using affine disturbance feedback as the basic form.

[0153] Within the RMPC framework, there is a type of tubular MPC that actively suppresses disturbances through an affine disturbance feedback strategy. Its common form is as follows:

[0154]

[0155] Where K is a fixed feedback gain, which ensures that the eigenvalues ​​of (A+BK) lie in the left half of the complex plane. These are the decision variables. Based on this classic idea, if the feedback gain can be changed and optimized within the MPC prediction range, the problem of traditional control strategies being unable to balance robustness and dynamic performance can be effectively solved. However, directly optimizing this feedback gain K in the state-feedback-based RMPC method leads to a non-convex problem. Therefore, this invention achieves convex synthesis by using a parameterization method.

[0156] For all k∈{t k ,t k +1,...,t k Within the MPC prediction range of length N, the control strategy parameterization is as follows: +N-1}

[0157]

[0158] Wherein, M(k|t k ) is time t k The planned feedback gain, v(k|t) k Both are auxiliary inputs and decision variables in the optimization problem, representing perturbations stacked along each prediction interval of the MPC. Defined as:

[0159]

[0160] Then, the predicted input sequences in formula (23) are stacked together to obtain the parameterized control strategy:

[0161]

[0162] For any time t k Matrix M(t) k ) and v(t) k All of these were obtained by arranging the decision variables as follows:

[0163]

[0164] Step 4: Based on the nominal form of the error system and considering the constraints in Steps 2 and 3, design the RMPC algorithm to achieve robust performance of the system.

[0165] Specifically, the forward Euler method is used, with a fixed step size δ, to discretize the nominal error system (7) and the actual error system (10):

[0166]

[0167] p e (t k ) = A k ·p e (t k-1)+B k ·u e (t k-1 )+R(θ f (t k-1 ))·(d(t k )-d(t k-1 (28)

[0168] in,

[0169] Let the prediction time domain of MPC be N, p e (k|t k ) indicates at time t k Given any possible perturbation, the predicted state quantity for the k-th prediction interval is obtained by predicting the control input sequence {u}. e (t k |t k ),u e (t k +1|t k ),...,u e (t k +N-1|t k The result is obtained when k = t in the actual error system (10). k +N uses terminal control input u Ω ;

[0170] After obtaining the predicted values ​​of each state variable, t is iteratively calculated based on the feasible set of perturbations constructed in step 2. k The prediction feasible sets {Θ} corresponding to the N prediction time domains at time points k (t k |t k ),Θ k (t k +1|t k ),......,Θ k (t k +N|t k These feasible sets of predictions are integrated into the prediction model in the form of constraints, forming a constrained nominal prediction model. This model not only explicitly considers the impact of disturbances on the feasible sets, but also dynamically limits the disturbance estimates to an allowable range at future times. The open-loop optimization was transformed into a rolling time-domain robust optimization.

[0171] The predicted values ​​of the control inputs are then used to constrain the control inputs in the form of equations based on the control strategy obtained in step 3. These constraints, based on affine disturbance feedback, consist of state variables, control variables, and estimated disturbances, directly reflecting the close relationship between disturbance dynamics, control strategy, and feasible sets. Through this connection, and by incorporating the planned feedback gain M into the decision variables using parameterization, the complex problem of robust constraint handling is transformed into a convex optimization problem, reducing the complexity of online computation.

[0172] Since MPC typically relies on high-precision models, and the actual error system contains disturbance terms and thus exhibits uncertainty, based on the discretized nominal error system, the cost function J that needs to be optimized in discrete-time model predictive control is given. * :

[0173]

[0174] in, These are the stage cost and the terminal cost, which are composed of discrete variables, respectively.

[0175] At each step, minimize the following optimization problem:

[0176]

[0177] Where Ω represents the terminal set of tracking errors, which is an invariant set. The perturbation estimate corresponding to the k-th prediction time domain.

[0178]

[0179] The above algorithm has been verified to be feasible through simulation experiments. The simulation results of trajectory tracking are shown in the figure below. Figure 3 As shown in the figure, the wheeled mobile robot can quickly adjust its pose to reach the desired position when it is far from the desired trajectory, and has good tracking accuracy.

[0180] Of course, the above description is not intended to limit the present invention, and the present invention is not limited to the examples given above. Any changes, modifications, additions or substitutions made by those skilled in the art within the scope of the present invention should also fall within the protection scope of the present invention.

Claims

1. A robust control method for trajectory tracking of a wheeled mobile robot based on feasible sets, characterized in that, Includes the following steps: Step 1: Establish the trajectory error system of the wheeled mobile robot. Specifically, this includes establishing an actual error system based on the deviation between the actual trajectory affected by disturbance and the set desired trajectory. Its state space description is denoted as the actual form of the error system. It also includes establishing a nominal error system based on the deviation between the nominal trajectory ignoring the influence of disturbance and the set desired trajectory. Its state space description is denoted as the nominal form of the error system. Step 2: Based on the actual form of the error system, construct the feasible set of perturbations in the system by combining the dynamic characteristics of the perturbation with the geometric evolution of the feasible set; Step 3: Parameterize the control strategy and design control input equality constraints based on feasible sets, using affine disturbance feedback as the basic form. Step 4: Based on the nominal form of the error system and considering the constraints in Steps 2 and 3, design the RMPC algorithm to achieve robust performance of the system.

2. The robust control method for trajectory tracking of a wheeled mobile robot based on feasible sets according to claim 1, characterized in that, In step 1, the desired trajectory and corresponding pose are considered as the virtual leader, and the actual trajectory is considered as the follower. The robot's "head position" is used as the trajectory coordinates during the movement, and its kinematic model is represented as follows: Where, the state variable ξ = [x yθ] Τ x and y represent the position information of the wheeled mobile robot along the x and y axes, respectively, and θ is the steering angle; the control input u = [vω] Τ v is the linear velocity, ω is the angular velocity; v and ω satisfy a rhombus constraint u∈U, This constraint can effectively prevent wheeled mobile robots from failing to operate normally due to a certain control variable being 0 during operation; Establish the Frenet-Serret coordinate system with the leader and followers as the organism centers. r O and f In the follower coordinate system, the coordinate deviation between the leader and the follower is the positional deviation between them. Transforming the leader's coordinates to the follower coordinate system yields the positional deviation: Where, p e =[x e y e ] Τ This represents the error between the follower and the leader, where h represents the distance from the center of the organism to the head, and x represents the error between the follower and the leader. r y r θ r Indicates the leader's pose information, x f y f θ f Indicates the pose information of the follower; Differentiate formula (2): Where, θ e =θ r -θ f v r v f ω represents the linear velocity of the leader and the follower, respectively. f Indicates the angular velocity of the follower; make Formula (3) can be simplified as follows: Formula (4) is the dynamic model of tracking error. Based on this, the trajectory error system of the wheeled mobile robot is established, which includes the actual error system based on the deviation between the actual trajectory affected by disturbance and the expected trajectory, and the nominal error system based on the deviation between the nominal trajectory and the expected trajectory ignoring the influence of disturbance. The nominal form of positional deviation is: in, A nominal form representing the positional deviation between followers and leaders. The nominal value for the follower's pose information; Differentiating formula (5) yields the nominal form of the error system: in, x e y e θ e v f ω f The nominal form; Formula (6) can be simplified as follows: Taking the disturbance d into account in the deviation, formula (2) becomes: Where, d x d y These represent disturbances in the x and y directions of the tracking trajectory, respectively. Taking the derivative of equation (8), we obtain the actual form of the error system as follows: make Equation (9) can be simplified to: Define {t k :k∈N,t k+1 -t k =δ}, δ>0 represents the sampling time series. An optimization problem is solved once at each sampling time, giving the cost function J that needs to be optimized in model predictive control. For the sake of model accuracy, this cost function J consists of nominal quantities: in, Indicates stage cost, Let T represent the terminal cost, Q, P, and R be positive definite matrices, and T be the prediction time domain, satisfying T = Nδ, N ∈ N, where δ represents the fixed step size.

3. The robust control method for trajectory tracking of a wheeled mobile robot based on feasible sets according to claim 2, characterized in that, Step 2 specifically involves: Assume that the disturbance in the actual form of the error system satisfies the condition of being unknown but bounded: in, Indicates the boundary of the disturbance; Furthermore, the dynamic characteristics of the perturbation d satisfy the Lipschitz continuity, i.e., there exists a Lipschitz constant L. d A value greater than 0 makes the rate of change of the disturbance globally bounded. This assumption transforms the discrete-time perturbation increment constraint into: in, Indicates the upper and lower bounds of the rate of change of the disturbance; Incorporating this disturbance into the actual form of the error system, i.e., formula (10), and using the forward Euler method, discretize formula (10) by letting: Then, formula (10) becomes: The disturbance change Δd(t) can be extracted from formula (14). k )=d(t k )-d(t k-1 ), further obtaining Δd(t) k The expression for ) is: Δd(t k )=R -1 (θ f (t k-1 ))(p e (t k )-A k p e (t k-1 )-B k u e (t k-1 )) (16) Comparing formulas (5) and (8), it can be seen that the disturbance d can be separated in formula (8), that is, the two formulas satisfy the following approximate relationship: Substituting formula (17) into formula (16), we get: in, Based on the assumptions mentioned in formula (13): Thus, the perturbation at t is obtained. k Feasible domain at time For any time t q , t q ≤t k The corresponding feasible region for: Finally, the feasible set Θ is obtained. k The expression: Where Ω0 is a custom initial feasible set.

4. The robust control method for trajectory tracking of a wheeled mobile robot based on feasible sets according to claim 3, characterized in that, Step 3 specifically involves: For all k∈{t k ,t k +1,...,t k Within the MPC prediction range of length N, the control strategy parameterization is as follows: +N-1} Wherein, M(k|t k ) is time t k The planned feedback gain, v(k|t) k Both are auxiliary inputs and decision variables in the optimization problem; the perturbations stacked along each prediction interval of the MPC are... Defined as: Then, the predicted input sequences in formula (22) are stacked together to obtain the parameterized control strategy: For any time t k Matrix M(t) k ) and v(t) k All of these were obtained by arranging the decision variables as follows:

5. A robust control method for trajectory tracking of a wheeled mobile robot based on feasible sets according to claim 4, characterized in that, Step 4 specifically involves: Using the forward Euler method and a fixed step size δ, the nominal error system (7) and the actual error system (10) are discretized. change: p e (t k )=A k ·p e (t k-1 )+B k ·u e (t k-1 )+R(θ f (t k-1 ))·(d(t k )-d(t k-1 )) (27) in, Let the prediction time domain of MPC be N, p e (k|t k ) indicates at time t k Given any possible perturbation, the predicted state quantity for the k-th prediction interval is obtained by predicting the control input sequence {u}. e (t k |t k ),u e (t k +1|t k ),...,u e (t k +N-1|t k The result is obtained when k = t in the actual error system (10). k +N uses terminal control input u Ω ; After obtaining the predicted values ​​of each state variable, t is iteratively calculated based on the feasible set of perturbations constructed in step 2. k The prediction feasible sets {Θ} corresponding to the N prediction time domains at time points k (t k |t k ),Θ k (t k +1|t k ),......,Θ k (t k +N|t k These feasible sets of predictions are integrated into the prediction model in the form of constraints, forming a constrained nominal prediction model; The predicted value of the control quantity is then used to constrain the control input in the form of an equation based on the control strategy obtained in step 3. At the same time, the planning feedback gain M is added to the decision variable by means of parameterization, and the complex robust constraint handling problem is transformed into a convex optimization problem. Based on the discrete nominal error system, the cost function J that needs to be optimized in discrete-time model predictive control is given. * : in, These are the stage cost and the terminal cost, which are composed of discrete variables, respectively. At each step, minimize the following optimization problem: Where Ω represents the terminal set of tracking errors, which is an invariant set. The perturbation estimate corresponding to the k-th prediction time domain.

Images