A trajectory tracking control method for mobile robots

Abstract

The present application relates to the field of robot control technology, and in particular to a mobile robot trajectory tracking control method. The method includes: determining the kinematic equation of a robot model under the control of a control system, setting a reference trajectory as a global reference trajectory, defining a trajectory tracking error based on the kinematic equation and the global reference trajectory, and establishing a position error model based on the trajectory tracking error; discretizing the kinematic equation and the position error model to obtain a kinematic discrete model and an error discrete model of the mobile robot; defining a cost function, setting the triggering conditions and triggering intervals of an event triggering mechanism based on the cost function, and solving the optimal control problem in a finite prediction time domain when the triggering conditions are met to obtain an optimal control sequence and a corresponding state trajectory. Through this application, the amount of online MPC calculations can be reduced and the adaptability and robustness of the control algorithm can be improved.

Description

Technical Field

[0001] The present application relates to the field of robot control technology, and in particular to a trajectory tracking control method for a mobile robot. Background Art

[0002] Mobile robot trajectory tracking is a core issue in the field of mobile robot control. Its goal is to enable the robot to move along a predetermined path (such as a straight line, arc, etc.). This usually involves defining the robot's kinematic and dynamic models and designing corresponding trajectory planning algorithms and controllers to achieve this goal, focusing on its trajectory tracking accuracy, economy and safety. At present, the mainstream trajectory tracking control algorithms include PID control, backstepping control, sliding mode control and LQR control (Linear Quadratic Regulator control, LQR). Although these algorithms have achieved remarkable results in many applications, they have certain limitations when dealing with multiple inequality constraints.

[0003] Model Predictive Control (MPC) has unique advantages in dealing with non-holonomic mobile robot constraints. Model Predictive Control (MPC) is an algorithm developed based on optimal control theory. It generates the optimal control sequence within a finite number of future time steps by solving an optimization problem with constraints at each moment. In actual operation, the system only executes the first control quantity in the sequence each time, and then re-rolls the optimization to generate a new control sequence based on the new state of the system. However, solving optimization problems online requires equipment with high computing power. In actual engineering environments, when hardware equipment conditions are limited, there is a risk of insufficient computing power or the control signal cannot be updated in time, resulting in system instability.

[0004] It should also be noted that mobile robots usually work in complex and changing environments, so they will inevitably encounter obstacles when tracking a given reference trajectory. Traditional obstacle avoidance methods, such as the A* algorithm, may not guarantee a global optimal solution. At the same time, the adjustment of parameters in the algorithm often relies on experience and trial and error, and there cannot be clear constraints on safety assurance, such as the control Lyapunov function method.

[0005] Currently, no effective solution has been proposed for how to reduce the amount of online computing in MPC and improve its applicability in resource-constrained environments. Summary of the invention

[0006] An embodiment of the present application provides a mobile robot trajectory tracking control method to at least reduce the amount of MPC online calculations.

[0007] To achieve the above objectives, the present invention provides a mobile robot trajectory tracking control method, comprising:

[0008] A model building step, based on a robot model, determining its kinematic equation under the control of the control system, setting a reference trajectory as a global reference trajectory, defining a trajectory tracking error based on the kinematic equation and the global reference trajectory, and building a position error model based on the trajectory tracking error;

[0009] A discrete processing step, discretizing the kinematic equation and the position error model to obtain a kinematic discrete model and an error discrete model of the mobile robot;

[0010] The optimal control step defines a cost function, sets the triggering condition and triggering interval of the event triggering mechanism based on the cost function, and solves the optimal control problem in the finite prediction time domain when the triggering condition is met to obtain the optimal control sequence and the corresponding state trajectory;

[0011] Wherein, the cost function is a function related to the error discrete model;

[0012] The trigger condition is configured as follows: , is an adjustable parameter, is the perturbation upper bound, is the Lipschitz constant, is the sampling period, For the The actual trajectory of the step open-loop control, For the The optimal state trajectory of the step open-loop control, the trigger event sequence is ,in It refers to the triggering moment;

[0013] The trigger interval is configured as: , sup{} is the supremum function.

[0014] In some embodiments, in the optimal control step, a cost function is defined based on the error discrete model, the cost function includes stage cost and terminal cost, and the cost function is expressed as the following calculation model:

[0015] ,

[0016] The stage cost is expressed as: , is the weighted sum of squares of trajectory state errors, is the weighted square sum of the control inputs. The terminal cost is expressed as: , The weighted sum of squares of the trajectory terminal state errors, , , , are the weight matrices of the cost function, respectively, where is the cost weight of the control input, is the cost weight of the terminal state error, is the cost weight of the stage state error, The length of the prediction horizon in model predictive control determines the time range for predicting future states, as well as the optimization time and computational complexity.

[0017] In some embodiments, in the trigger event sequence, the triggering moment satisfies:

[0018] , where the trigger interval Used to indicate the time during which the optimal control variable at the previous moment continues to act.

[0019] In some embodiments, the optimal control problem is expressed as the following computational model:

[0020] in:

[0021] in, is the predicted starting state error and the current actual error Consistency;

[0022] Indicates that the prediction state variables are initialized using the current state;

[0023] To predict the time The state error, To predict the time The state error of

[0024] Indicates that the state at the next moment is updated by the state at the previous moment through the system function, that is, through the state transfer function and control input to update the state error;

[0025] To predict the time The control input, Express the input constraints to ensure that the input does not exceed the system's tolerance range and cause system instability. U is the control input constraint condition.

[0026] is the terminal constraint, Defined as The error allowable region with a radius is a constraint that ensures that the terminal state enters the terminal region when the MPC prediction horizon ends.

[0027] In some embodiments, the optimal control sequence and the corresponding state trajectory It is expressed as the following calculation model:

[0028] ,

[0029] .

[0030] In some embodiments, the prediction time domain is The size of the moment is based on the following adaptive prediction time domain The calculation model is obtained.

[0031] ,

[0032] in, is the minimum prediction time domain, is the variable prediction horizon, is the initial prediction time domain.

[0033] In some embodiments, the adaptive prediction time domain satisfies the constraint condition:

[0034] .

[0035] Based on the above steps, the present application can flexibly adjust according to different control requirements and environmental changes through adaptive prediction time domain, thereby improving the adaptability and robustness of the control algorithm. By reducing the prediction time domain, the dimension and computational complexity of the optimization problem can be reduced, thereby improving the real-time performance of the control algorithm.

[0036] In some embodiments, a discrete control obstacle function Introducing the cost function, the cost function is expressed as the following calculation model:

[0037] ,

[0038] in,

[0039] ,

[0040] Satisfy the constraints: , is an adjustable parameter, usually used to adjust the weight of the penalty term. is the discrete control barrier function The number of obstacles is used to indicate the number of is the discrete control barrier function The maximum allowed value.

[0041] In some embodiments, the discrete control obstacle function The MPC framework is introduced to solve the optimal control problem, and the optimal control problem is expressed as the following calculation model:

[0042]

[0043] in,

[0044] , ,

[0045] , For safety tolerance distance, is the maximum radius of the obstacle.

[0046] In some embodiments, the obstacle avoidance process is set as a trigger condition of the event trigger mechanism, and the trigger interval optimization configuration is:

[0047] .

[0048] Based on the above steps, this application introduces CBF (Control Barrier Function) to enable the control algorithm to effectively avoid collision with obstacles while ensuring trajectory tracking accuracy.

[0049] Details of one or more embodiments of the present application are set forth in the following drawings and description to make other features, objects, and advantages of the present application more readily apparent. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] The drawings described herein are used to provide a further understanding of the present application and constitute a part of the present application. The illustrative embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0051] Figure 1 is a flow chart of a trajectory tracking control method according to an embodiment of the present application;

[0052] Figure 2 is a schematic diagram of a mobile robot model according to an embodiment of the present application;

[0053] Figure 3 is a schematic diagram of the corresponding relationship between the trigger interval and the prediction time domain according to an embodiment of the present application;

[0054] Figure 4 It is a schematic diagram of the trigger frequency of the control method according to the prior art;

[0055] Figure 5 is a schematic diagram of triggering frequency of a trajectory tracking control method according to an embodiment of the present application;

[0056] Figure 6 It is a schematic diagram of the principle of the trajectory tracking control method according to the third embodiment of the present application. DETAILED DESCRIPTION

[0057] In order to make the purpose, technical solutions and advantages of the present application clearer, the present application is described and illustrated below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not intended to limit the present application. Based on the embodiments provided in the present application, all other embodiments obtained by ordinary technicians in the field without making creative work are within the scope of protection of the present application.

[0058] Obviously, the drawings described below are only some examples or embodiments of the present application. For ordinary technicians in this field, the present application can also be applied to other similar scenarios based on these drawings without creative work. In addition, it can also be understood that although the efforts made in this development process may be complicated and lengthy, for ordinary technicians in this field related to the content disclosed in this application, some changes in design, manufacturing or production based on the technical content disclosed in this application are just conventional technical means, and should not be understood as insufficient content disclosed in this application.

[0059] MPC uses an existing model, the current state of the system and future control quantities to predict the future output of the system, and then compares it with the expected system output to obtain a cost function, also called a loss function or cost function.

[0060] Optimal control refers to the system performance that reaches the optimal state under certain constraints, where the constraints are usually the limitations imposed by the actual environment. Generally, optimal control needs to be optimized over the entire time domain (integral from time 0 to positive infinity) to ensure optimality. This is a very greedy behavior that consumes a lot of computing power. At the same time, if the system is a time-varying system or faces disturbances, the optimal value obtained at the previous moment may not necessarily be the optimal value at the next moment. MPC only considers the future. time steps.

[0061] Embodiment 1:

[0062] The embodiment of the present application is based on the traditional MPC framework. Considering the problem of large computational complexity of the traditional model predictive control method under the condition of limited computing resources, a mobile robot trajectory tracking control method based on an event trigger mechanism is designed to reduce the number of optimization solutions and improve computing efficiency. Figure 1 This is a flow chart of the trajectory tracking control method of the present application embodiment, refer to Figure 1 As shown, the method comprises the following steps:

[0063] Model building step S1, determining the kinematic equation of a robot model under the control of a control system, setting a reference trajectory as a global reference trajectory, defining a trajectory tracking error based on the kinematic equation and the global reference trajectory, and building a position error model based on the trajectory tracking error;

[0064] refer to Figure 2 As shown, the global coordinates of the above robot model are , the local coordinates are , Represents the robot's center of mass Geometric center of the driving wheel The distance between the two wheels is half the wheelbase. ,The global coordinate system in the above figure is used to describe the position of the robot in the entire environment, and the local coordinate system is used to describe the position of the robot relative to its own reference point.

[0065] Therefore, the kinematic equation of the mobile robot is:

[0066] ,

[0067] in, is the system state of the mobile robot, is the control input, represents the linear velocity of the mobile robot, represents the angular velocity of the mobile robot, For bounded disturbances, the linear velocity and angular velocity constraints of the mobile robot in the embodiment of the present application are configured as follows: , , the configuration control input constraints are , is the maximum value of the angular velocity and linear velocity of the mobile robot.

[0068] It should be noted that the nonlinear function About the status Locally Lipschitz continuous, then the Lipschitz constant , It is an adjustable parameter.

[0069] The above reference trajectory is expressed as The goal of trajectory tracking control of a mobile robot is to track the reference trajectory so that the trajectory tracking deviation converges to 0.

[0070] Based on the reference trajectory and the motion trajectory of the mobile robot, the trajectory tracking error is expressed as:

[0071] ,

[0072] Define the state error as Represents the deviation between the actual position of the robot and the expected position, and the control input is defined as ,in are the reference linear velocity and reference angle, is the reference angular velocity, is the reference angle, and the reference linear velocity, reference angular velocity and reference angle constitute the reference trajectory. .

[0073] Then the position error model can be obtained as:

[0074] ,

[0075] Discretization step S2, using the forward Euler method to discretize the kinematic equation and the position error model, to obtain a kinematic discrete model and an error discrete model of the mobile robot; specifically:

[0076] The kinematic equations of the mobile robot are discretized using the forward Euler method to obtain Moment kinematics discrete model:

[0077] ,

[0078] The position error model is discretized using the forward Euler method to obtain Moment error discrete model:

[0079] ,

[0080] In the above formula (4) and formula (5), is the sampling period.

[0081] The optimal control step S3 defines a cost function, sets the triggering condition and triggering interval of the event triggering mechanism based on the cost function, and solves the optimal control problem in the finite prediction time domain when the triggering condition is met to obtain the optimal control sequence and the corresponding state trajectory;

[0082] Wherein, the cost function is a function related to the error discrete model;

[0083] The trigger condition is configured as follows: ,

[0084] in, is an adjustable parameter, is the perturbation upper bound, is the Lipschitz constant, is the sampling period, For the The actual trajectory of the step open-loop control, For the The optimal state trajectory of the step open-loop control, the trigger event sequence is ,in It refers to the triggering moment;

[0085] The trigger interval is configured as: , sup{} is the supremum function.

[0086] In the optimal control step S3, a cost function is defined based on the error discrete model. The cost function includes stage cost and terminal cost. The cost function is expressed as the following calculation model:

[0087] ,

[0088] The stage cost is expressed as: ,

[0089] is the weighted sum of squares of trajectory state errors, The terminal cost is the weighted sum of squares of the control inputs and is expressed as: , The weighted sum of squares of the trajectory terminal state errors, , , , are the weight matrices of the cost function, is the predicted time domain length.

[0090] refer to Figure 3 As shown, in the above trigger event sequence, the trigger time satisfies , where the trigger interval Used to indicate the time during which the optimal control variable at the previous moment continues to act.

[0091] In the above embodiment, the optimal control problem is expressed as the following calculation model:

[0092]

[0093] in:

[0094] is the predicted starting state error and the current actual error Consistency;

[0095] Indicates that the prediction state variables are initialized using the current state;

[0096] To predict the time The state error, To predict the time The state error of

[0097] Indicates that the state at the next moment is updated by the state at the previous moment through the system function, that is, through the state transfer function and control input to update the state error;

[0098] To predict the time The control input, Express the input constraints to ensure that the input does not exceed the system's tolerance range and cause system instability. U is the control input constraint condition.

[0099] is the terminal constraint, Defined as The error allowable region with radius is a constraint to ensure that the terminal state enters the terminal region when the MPC prediction time domain ends. It is an adjustable parameter. The smaller the value, the higher the tracking accuracy. The terminal constraint means that during the optimization process, the constraint condition applies not only to each moment in the prediction time domain, but also to the final moment of the control process, which is usually the last step (i.e., the terminal) of the prediction time domain. We give the terminal constraint a range to ensure that the system can still maintain the stability, feasibility, and reliability of the system after the prediction time domain ends.

[0100] In some embodiments, the optimal control sequence and the corresponding state trajectory It is expressed as the following calculation model:

[0101] ,

[0102] .

[0103] For the configuration of trigger conditions and trigger intervals, based on The error between the actual trajectory and the optimal trajectory of the step open-loop control is inferred, specifically:

[0104] The error between the actual trajectory and the optimal trajectory of the step open-loop control is expressed as:

[0105] ,

[0106] in, .

[0107] According to the discrete Gronwall-Bellman-Ou-Iang-type inequality, we can get:

[0108] .

[0109] For an open-loop control system, there will be a certain error between the actual trajectory and the optimal trajectory. When the system satisfies the Lipschitz continuity, the following are set in the embodiment of the present application:

[0110] The trigger conditions are: ,

[0111] The resulting trigger interval is: .

[0112] Based on the above steps, the present application adopts an event trigger mechanism based on state error, and solves the optimal control problem only when the trigger condition based on state error occurs, which greatly reduces the frequency of solving the optimization problem, avoids the high-frequency optimization solution caused by the traditional MPC periodic update, and improves the solution operation efficiency.

[0113] Embodiment 2:

[0114] In MPC, the prediction horizon determines the future time range considered in the optimization problem. The size of the prediction horizon is dynamically adjusted according to the current state of the mobile robot and the surrounding environment, which can balance the computational complexity and control performance. In order to further reduce the computational complexity of the embodiment of the present application and reduce the computational burden of MPC, this embodiment designs an adaptive prediction horizon based on the dual feedback of position error and trajectory curvature on the basis of the embodiment. .

[0115] Specifically, the prediction time domain is The size of the moment is based on the following adaptive prediction time domain The calculation model is obtained.

[0116] ,

[0117] in, is the minimum prediction time domain, is the variable prediction horizon, is the initial prediction time domain.

[0118] Considering that a too small prediction time domain may make the system unstable, it is necessary to ensure that the terminal tracking error enters the terminal area at the end of the prediction time domain. Therefore, in the above embodiment, the minimum prediction time domain should meet the following conditions: ,in For the time step Any The optimal prediction error state obtained by step is The state error at the moment is within a range of the error in the i-th step, and this condition is used to determine the size of the minimum prediction time domain.

[0119] In the above embodiment, the size of the variable time domain is determined based on the curve curvature, the tracking error size and the error change rate at the last moment of the previous trigger moment, and the functional relationship between them is represented by the O function:

[0120] ,

[0121] in, is the maximum tracking error; , and is the weight coefficient; is the trajectory curvature; is the error change rate, which is used to characterize the error change trend.

[0122] In some embodiments, the adaptive prediction time domain satisfies the constraint condition: Further, in order to ensure the iterative feasibility of the closed-loop system and to ensure that the optimization problem has a solution, the constraint is added: .

[0123] Based on the above embodiments, the present application can flexibly adjust according to different control requirements and environmental changes through adaptive prediction time domain, thereby improving the adaptability and robustness of the control algorithm. By reducing the prediction time domain, the dimension and computational complexity of the optimization problem can be reduced, thereby improving the real-time performance of the control algorithm.

[0124] Embodiment three:

[0125] In order to limit the movement range of the robot to avoid the mobile robot from colliding with obstacles, this embodiment further introduces an obstacle avoidance constraint function into the method based on the second embodiment.

[0126] Define an invariant set C, which is defined as a continuously differentiable function The super level set of is expressed as follows: ;

[0127] Define the obstacle area as ,in, is the effective area of ​​robot operation, is the entire state space, which is used to describe a vector space with n real number elements. It can be understood as an n-dimensional coordinate space, in which each point is represented by n real number coordinates. Indicates the area where the robot may collide with obstacles;

[0128] for , And there is an extension class function ,

[0129] satisfy: .

[0130] Based on this, the discrete control obstacle function that satisfies the following obstacle avoidance constraints is obtained: :

[0131] ,

[0132] in, and , here Defined as a scalar.

[0133] Based on this, the discrete control obstacle function Introducing the cost function, the cost function is expressed as the following calculation model:

[0134] ,

[0135] in,

[0136] ,

[0137] , is an adjustable parameter, usually used to adjust the weight of the penalty term. for The number of obstacles is used to indicate the number of yes The maximum allowed value.

[0138] Similarly, the discrete control obstacle function (DCBF) introduces the MPC framework to solve the optimal control problem, and the optimal control problem is expressed as the following calculation model:

[0139]

[0140] in,

[0141] , ,

[0142] , For safety tolerance distance, is the maximum radius of the obstacle.

[0143] Finally, in order to ensure the safety during the obstacle avoidance process, the obstacle avoidance process is set as the trigger condition of the event trigger mechanism, and the trigger interval optimization configuration is:

[0144] .

[0145] Based on the above configuration, we can get Figure 6 The adaptive predictive time domain EMPC-DCBF controller suitable for differential mobile robots shown in the figure uses a solver to perform the above-mentioned cost function, obstacle avoidance cost based on discrete control obstacle function, constraint conditions of the above-mentioned embodiment, DCBF solution and adaptive predictive time domain solution, and executes the above-mentioned event triggering system through an event triggering controller.

[0146] Based on the above embodiments, the introduction of DCBF enables the control algorithm to effectively avoid collisions with obstacles while ensuring trajectory tracking accuracy, meet safety constraints, and adjust the prediction time domain according to the dynamic characteristics and real-time conditions of the system, so that the control strategy fits the actual system behavior, thereby improving the accuracy of trajectory tracking and the stability of the system. At the same time, the control parameters can be dynamically adjusted according to changes in the state of the mobile robot and changes in the external environment, so that the mobile robot can quickly respond to various changes, enhancing the adaptability of the mobile robot.

[0147] In order to verify the technical effect of this embodiment compared with the existing control method, a comparative experiment was conducted. Figure 4 , Figure 5 As shown, the number of triggering times of the existing control method is 1667 times, and the number of triggering times of this embodiment is 444 times; the operation time of the existing control method is 7896.573 s; the operation time of this embodiment is 873.960 s. Among the 1667 optimization problems, this embodiment only solves the optimization problems 444 times, the number of solutions is reduced by 73.36%, and the total time cost is reduced by 88.93%.

[0148] In summary, the embodiments of the present application achieve the goal of reducing the computational burden and improving the solution speed while ensuring the control performance, thereby reducing the waiting time for control commands, improving the response speed, and effectively improving the real-time control work of the mobile robot.

[0149] The technical features of the above-described embodiments may be arbitrarily combined. To make the description concise, not all possible combinations of the technical features in the above-described embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0150] The above-mentioned embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the patent of the present application shall be subject to the attached claims.

Claims

1. A mobile robot trajectory tracking control method, characterized in that: include: A model building step, based on a robot model, determining its kinematic equation under the control of the control system, setting a reference trajectory as a global reference trajectory, defining a trajectory tracking error based on the kinematic equation and the global reference trajectory, and building a position error model based on the trajectory tracking error; A discrete processing step, discretizing the kinematic equation and the position error model to obtain a kinematic discrete model and an error discrete model of the mobile robot; The optimal control step defines a cost function, sets the triggering condition and triggering interval of the event triggering mechanism based on the cost function, and solves the optimal control problem in the finite prediction time domain when the triggering condition is met to obtain the optimal control sequence and the corresponding state trajectory, wherein the cost function includes the stage cost and the terminal cost; Wherein, the cost function is a function related to the error discrete model; The trigger condition is configured as follows: , is an adjustable parameter, is the perturbation upper bound, is the Lipschitz constant, is the sampling period, For the The actual trajectory of the step open-loop control, For the The optimal state trajectory of the step open-loop control, the trigger event sequence is ,in It refers to the triggering moment; A discrete control barrier function Introducing the cost function, the cost function is expressed as the following calculation model: ， The stage cost is expressed as: ; is the weighted sum of squares of trajectory state errors, The terminal cost is the weighted sum of squares of the control inputs and is expressed as: , is the weighted sum of squares of the trajectory terminal state errors, , , , are the weight matrices of the cost function, To predict the time domain length; ， , is an adjustable parameter used to adjust the weight of the penalty term. for The number of yes The maximum permissible value of The discrete control obstacle function The MPC framework is introduced to solve the optimal control problem, and the optimal control problem is expressed as the following calculation model: in, is the predicted starting state error and the current actual error Consistency; Indicates that the prediction state variables are initialized using the current state; To predict the time The state error, To predict the time The state error of Indicates that the state at the next moment is updated by the state at the previous moment through the system function, that is, through the state transfer function and control input to update the state error; To predict the time The control input, represents the input constraint, ensuring that the input does not exceed the system's tolerance range and cause system instability. U is the control input constraint condition. is the terminal constraint, ， ， , For safety tolerance distance, is the maximum radius of the obstacle; In the trigger event sequence, the trigger time satisfies , where the trigger interval It is used to represent the continuous action time of the optimal control quantity at the previous moment, and the obstacle avoidance process is set as the trigger condition of the event trigger mechanism. The trigger interval optimization configuration is: , sup{} is the supremum function.

2. The mobile robot trajectory tracking control method according to claim 1, characterized in that: The optimal control sequence and the corresponding state trajectory It is expressed as the following calculation model: ， 。 3. The mobile robot trajectory tracking control method according to claim 1, characterized in that: The prediction time domain is The size of the moment is based on the following adaptive prediction time domain The calculation model is obtained. ， in, is the minimum prediction time domain, is the variable prediction horizon, is the initial prediction time domain.

4. The mobile robot trajectory tracking control method according to claim 3, characterized in that: The adaptive prediction time domain satisfies the constraint condition: .

Images