Mobile Robot Trajectory Tracking Control Method, Device and Medium Based on High-Order Full-Drive Theory

Abstract

The present invention belongs to the field of robot motion control, and discloses a trajectory tracking control method, device and medium for a mobile robot based on the high-order fully actuated theory, including: constructing a two-wheeled bicycle model of the mobile robot and converting the state space model into a fully actuated model; designing a fully actuated stabilizing controller for the trajectory tracking control problem of the two-wheeled bicycle model of the mobile robot to obtain a trajectory tracking control system error model of the mobile robot; according to the trajectory tracking control system error model of the mobile robot, introducing a system increment equation to simplify the mathematical model; designing an MPC controller to solve the optimal control sequence, and obtaining the final input of the motion control system of the mobile robot through inverse transformation. The present invention uses the high-order fully actuated theory to convert a complex state space model into a more concise second-order fully actuated model, which not only retains the physical meaning of the system, but also avoids the ill-conditioned matrix problem that may be encountered in traditional methods, greatly simplifying the controller design process.

Description

Technical Field

[0001] The present invention relates to the field of robot motion control, and particularly to a trajectory tracking control method, device and medium for a mobile robot based on the high-order fully actuated theory. Background Technique

[0002] With the rapid development of the robot industry and the rapid growth of its related modules, various robots have been widely used in military and civilian fields. As an important branch in the robot industry, the performance and technical level of mobile robots have also made remarkable progress. When performing tasks such as autonomous inspection and fixed-point material supply, such robots need to travel along a preset path. Therefore, it is of great significance to study the trajectory tracking problem of mobile robot systems.

[0003] Trajectory tracking means that a mobile robot starts from a specific initial state and precisely follows a reference trajectory that changes with time. Affected by factors such as model uncertainty, sensor noise, and external disturbances, the design of path tracking controllers is challenging. For this reason, technologies such as reinforcement learning, sliding mode control, robust control, and model predictive control (MPC) have been proposed to handle the trajectory tracking problem of mobile robots. Among them, MPC models based on the characteristics of the controlled object and makes decisions by predicting the future state change trend, with good feedback correction ability and robustness, thus becoming an effective method to solve the trajectory tracking problem of mobile robots.

[0004] It should be noted that most of the above methods still use state space models to describe networked control motion systems, which leads to the loss of the physical meaning of the original system and the appearance of ill-conditioned matrices in the model simplification process. In recent years, the fully actuated control system theory directly based on physical laws has been developed. As an extension of the traditional state space model theory, it not only retains the physical meaning of the original system but also prevents the possible ill-conditioned matrix problem in the simplification process, so it can represent the actual system more accurately. In addition, it provides a more convenient method for the design of controllers, which helps to improve the trajectory tracking accuracy and efficiency of mobile robots. Summary of the Invention

[0005] The purpose of the present invention is to provide a trajectory tracking control method, device and medium for a mobile robot based on the high-order fully actuated theory to solve the problems raised in the above background technique.

[0006] To achieve the above purpose, the present invention provides the following technical solutions:

[0007] A trajectory tracking control method for a mobile robot based on the high-order fully actuated theory, including:

[0008] Step S1: Construct a mobile robot two-wheeled bicycle model and convert the state space model into a full drive model;

[0009] Step S2: Design a full drive stabilizing controller for the trajectory tracking control problem of the mobile robot two-wheeled bicycle model to obtain the mobile robot trajectory tracking control system error model under the high-order full drive model;

[0010] Step S3: According to the mobile robot trajectory tracking control system error model given in Step S2, introduce the system increment equation to simplify the mathematical model;

[0011] Step S4: Design an MPC controller to solve the optimal control sequence and obtain the final input of the mobile robot motion control system through inverse transformation.

[0012] Furthermore, the Step S1 includes:

[0013] The state equation of the mobile robot two-wheeled bicycle model is as follows:

[0014] (1)

[0015] Wherein, respectively represent the abscissa, ordinate and attitude angle of the mobile robot, respectively represent the first-order derivatives of, respectively represent the speed and acceleration of the mobile robot, and the angle between the speed direction and the vehicle body direction is , represents the distance between the front wheel and the rear wheel of the mobile robot;

[0016] Perform a second derivative on :

[0017] (2)

[0018] Where respectively represent the second-order derivatives of, denote the intermediate variable , design the control quantity , substitute it into Equation (2) to obtain the following state space equation:

[0019] (3)

[0020] Where represents the second-order derivative of, discretize Equation (3) according to the sampling time to obtain the following discrete state space equation:

[0021] (4)

[0022] Among them, represents the th sampling time, respectively represent the sampling time values, respectively represent coefficients, , represents the fully actuated stabilizing controller to be designed.

[0023] Furthermore, the step S2 includes:

[0024] Design the fully actuated stabilizing controller as follows:

[0025] (5)

[0026] Among them, respectively represent reference trajectories at sampling times, and respectively represent tracking errors at sampling times, represents the external control input, is the parameter matrix of the designed fully actuated stabilizing controller;

[0027] Substitute Equation (4) into Equation (3) to obtain the following high-order fully actuated tracking error control model in closed-loop form:

[0028] (6)

[0029] Among them is an identity matrix with appropriate order.

[0030] Furthermore, the step S3 includes:

[0031] Denote the intermediate variable , , then the high-order fully actuated tracking error control model shown in Equation (6) can be rewritten as:

[0032] (7)

[0033] Among them, is the system parameter matrix, where is an identity matrix with appropriate order;

[0034] Denote the intermediate variables , , , and the incremental equation of Equation (7) can be obtained as follows:

[0035] (8)

[0036] Record the intermediate variable , , there is:

[0037] (9)

[0038] where .

[0039] Furthermore, the step S4 includes:

[0040] Set the prediction interval N = K, and the recurrence expression is as follows:

[0041] (10)

[0042] where respectively represent at time recurrence;

[0043] Define the intermediate variable , where in the " " represents the omitted , then the formula (10) can be written in matrix form:

[0044] (11)

[0045] where respectively represent of power;

[0046] Define the quadratic performance index as:

[0047] (12)

[0048] where, , respectively represent transpose, represents the state weight matrix in diagonal form, represents the control weight matrix in diagonal form;

[0049] Solve the quadratic performance index through the Yalmip toolbox to obtain the optimal control sequence ; Take the first element of the sequence as the system control law at the current moment, and according to and , the obtained by the MPC controller is inverse-transformed into , according to to obtain the corresponding control quantity, and realize the trajectory tracking predictive control of the mobile robot.

[0050] The present invention also provides a mobile robot trajectory tracking predictive control device based on the high-order fully actuated theory, including one or more processors, which are used to implement a mobile robot trajectory tracking control method based on the high-order fully actuated theory as described above.

[0051] The present invention also provides a readable storage medium, on which a program is stored. When the program is executed by a processor, it realizes a mobile robot trajectory tracking control method based on the high-order fully actuated theory as described above.

[0052] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0053] 1) Using the high-order fully actuated theory to convert the complex state space model into a more concise second-order fully actuated model not only retains the physical meaning of the system but also avoids the ill-conditioned matrix problem that may be encountered in traditional methods, thus greatly simplifying the controller design process and improving the design efficiency.

[0054] 2) By constructing an error model, the deviation between the actual trajectory and the desired trajectory of the mobile robot can be captured more accurately, enabling the control system to adjust its behavior more accurately, and thus achieving higher trajectory tracking accuracy and control performance.

[0055] 3) Introducing the system increment equation to simplify the mathematical model reduces the amount of calculation and the required processing time, which not only helps to improve the real-time response speed but also makes the algorithm easier to deploy in resource-constrained environments.

[0056] 4) A MPC method based on the high-order fully actuated model is proposed to solve the optimal control law. Through the corresponding inverse transformation relationship, these control laws are converted back into control instructions applicable to the fully actuated system, and finally, the trajectory tracking predictive control of the mobile robot based on the high-order fully actuated theory is realized. Description of the Drawings

[0057] Figure 1 is a flowchart of a mobile robot trajectory tracking control method based on the high-order fully actuated theory of the present invention.

[0058] Figure 2 is a comparison curve graph of the reference trajectory and the actual motion trajectory of the mobile robot in a mobile robot trajectory tracking control method based on the high-order fully actuated theory of the present invention.

[0059] Figure 3 is a schematic structural diagram of a mobile robot trajectory tracking predictive control device based on the high-order fully actuated theory of the present invention. Detailed Embodiments

[0060] Next, in combination with the accompanying drawings in the embodiments of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0061] An embodiment of the present application proposes a trajectory tracking control method for a mobile robot based on the high-order full-drive theory, including the following steps:

[0062] Step S1: Construct a two-wheeled bicycle model of the mobile robot, design a full-drive control law, and convert the state space model into a full-drive model:

[0063] The state equation of the two-wheeled bicycle model of the mobile robot is as follows:

[0064] (1)

[0065] Among them, respectively represent the abscissa, ordinate and attitude angle of the mobile robot, respectively represent the first-order derivatives of, respectively represent the speed and acceleration of the mobile robot, the angle between the speed direction and the vehicle body direction is , represents the distance between the front wheel and the rear wheel of the mobile robot, .

[0066] Perform a second-order derivative on :

[0067] (2)

[0068] Among them respectively represent the second-order derivatives of, denote the intermediate variable , design the control quantity , substitute it into Equation (2) to obtain the following state space equation:

[0069] (3)

[0070] Among them represents the second-order derivative of, discretize Equation (3) according to the sampling time , , and obtain the following discrete state space equation:

[0071] (4)

[0072] Among them, represents the th sampling time, respectively represent the sampling time value, respectively represent coefficient, represents the fully actuated stabilizing controller to be designed. Among them, , .

[0073] Step S2: For the mobile robot's bicycle model with two wheels given by Equation (3), design a fully actuated stabilizing controller for its trajectory tracking control problem, and obtain the trajectory tracking control system error model of the mobile robot under the high-order fully actuated model. The fully actuated stabilizing controller is as follows:

[0074] (5)

[0075] Among them, respectively represent the reference trajectory at the sampling time, and respectively represent the tracking error at the sampling time, represents the external control input, is the parameter matrix of the designed fully actuated stabilizing controller. Configure the poles of the closed-loop system as , and obtain .

[0076] Substitute Equation (4) into Equation (3) to obtain the following closed-loop form of the high-order fully actuated tracking error control model:

[0077] (6)

[0078] Among them is an identity matrix with an appropriate order.

[0079] Step S3: According to the trajectory tracking control system error model of the mobile robot given in Step S2, introduce the system increment equation to simplify the mathematical model:

[0080] Denote the intermediate variable , , then the high-order fully actuated tracking error control model shown in Equation (6) can be re-expressed as:

[0081] (7)

[0082] Among them, is the system parameter matrix, where is an identity matrix with an appropriate order. .

[0083] Denote the intermediate variable , , , the incremental equation of Equation (7) can be obtained as follows:

[0084] (8)

[0085] Denote the intermediate variable , , there is:

[0086] (9)

[0087] Where , specifically as follows:

[0088] .

[0089] Step S4: Design an MPC controller to solve the optimal control sequence, and obtain the final input of the mobile robot motion control system through inverse transformation, including the following steps:

[0090] Set the prediction interval , N can be other numbers, and the recurrence expression is as follows:

[0091] (10)

[0092] Where respectively represent at time recurrence.

[0093] Define the intermediate variable , where in the " " represents the omitted and , then Equation (10) can be written in matrix form:

[0094] (11)

[0095] Where respectively represent the 2nd, 3rd, 4th, and 5th powers of

[0096]

[0097]

[0098]

[0099]

[0100]

[0101] Define the quadratic performance index as follows:

[0102] (12)

[0103] where , respectively represent the transpose of , represents the state weight matrix in diagonal form , represents the control weight matrix in diagonal form

[0104] Solve the quadratic performance index through the Yalmip toolbox to obtain the optimal control sequence . Take the first element of the sequence as the system control law at the current moment, and according to and , inverse-transform the obtained by the MPC controller into , and obtain the corresponding control quantity according to to achieve the trajectory tracking predictive control of the mobile robot

[0105] See Figure 3 , an embodiment of a mobile robot trajectory tracking predictive control device based on the high-order full drive theory provided by the present invention includes one or more processors for implementing a mobile robot trajectory tracking control method based on the high-order full drive theory in the above embodiment

[0106] An embodiment of a mobile robot trajectory tracking predictive control device based on the high-order full drive theory of the present invention can be applied to any device with data processing capabilities. The any device with data processing capabilities can be a device or apparatus such as a computer. The device embodiment can be implemented by software, or by hardware or a combination of software and hardware. Taking software implementation as an example, as a logically meaningful device, it is formed by the processor of any device with data processing capabilities reading the corresponding computer program instructions in the non-volatile memory into the memory and running. From the hardware level, as Figure 3 shown, it is a hardware structure diagram of any device with data processing capabilities where a mobile robot trajectory tracking predictive control device based on the high-order full drive theory of the present invention is located. Except for Figure 3In addition to the processor, memory, network interface, and non-volatile memory shown, any manufacturing device with data processing capabilities where the device in the embodiment is located may also include other hardware according to the actual functions of the any device with data processing capabilities, which will not be elaborated here.

[0107] The implementation processes of the functions and roles of each unit in the above device are specifically described in the implementation processes of the corresponding steps in the above method, which will not be elaborated here.

[0108] The technical features of the above-described embodiments can be combined arbitrarily. For the sake of brevity of description, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, it should be considered as the scope described in this specification.

[0109] The embodiment of the present invention also provides a readable storage medium, on which a program is stored. When the program is executed by a processor, it implements a mobile robot trajectory tracking control method based on the high-order full-drive theory in the above embodiment.

[0110] The readable storage medium may be an internal storage unit of any device with data processing capabilities described in any of the foregoing embodiments, such as a hard disk or memory. The readable storage medium may also be an external storage device, such as a plug-in hard disk, a Smart Media Card (SMC), an SD card, a Flash Card, etc. equipped on the device. Further, the readable storage medium may also include both the internal storage unit of any device with data processing capabilities and the external storage device. The readable storage medium is used to store the computer program and other programs and data required by any device with data processing capabilities, and may also be used to temporarily store the data that has been output or will be output.

[0111] Although the embodiments of the present invention have been shown and described, for those of ordinary skill in the art, it can be understood that various changes, modifications, substitutions, and variations can be made to these embodiments without departing from the principles and spirit of the present invention. The scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A mobile robot trajectory tracking control method based on high-order full-drive theory, characterized in that: include: Step S1: construct a mobile robot two-wheel bicycle model, and convert the state space model into an all-wheel drive model, including: The state equation of the mobile robot two-wheel bicycle model is as follows: (1) in, Respectively represent the horizontal coordinate, vertical coordinate and attitude angle of the mobile robot, Respectively The first derivative of They represent the speed and acceleration of the mobile robot respectively, and the angle between the speed direction and the body direction is , Represents the distance between the front and rear wheels of the mobile robot; right Take the second derivative: (2) in Respectively The second derivative of , design control quantity , substituting into equation (2) yields the following state space equation: (3) in express The second-order derivative of After discretization, the discrete state space equation is obtained as follows: (4) in, Indicates The sampling time, Respectively represent Sampling time The value of Respectively The coefficient of , It indicates that an all-wheel drive stabilization controller needs to be designed; Step S2, designing an all-wheel drive stabilization controller for the trajectory tracking control problem of the mobile robot two-wheel bicycle model, and obtaining an error model of the mobile robot trajectory tracking control system under the high-order all-wheel drive model, including: Design the all-wheel drive stabilization controller as follows: (5) in, Respectively The reference trajectory at the sampling time, and Respectively The tracking error at the sampling time, Represents external control input, is the parameter matrix of the designed all-wheel drive stabilization controller; Substituting equation (4) into equation (3), the closed-loop high-order all-wheel drive tracking error control model is obtained as follows: (6) in is the identity matrix of suitable order; Step S3: According to the mobile robot trajectory tracking control system error model given in step S2, the system increment equation is introduced to simplify the mathematical model, including: Remember the intermediate variables , , then the high-order all-wheel drive tracking error control model shown in equation (6) can be re-expressed as: (7) in, is the system parameter matrix, where is the identity matrix of suitable order; Remember the intermediate variables , , , the incremental equation of formula (7) can be obtained as follows: (8) Remember the intermediate variables , ,have: (9) in ; Step S4: Design an MPC controller to solve the optimal control sequence, and obtain the final input of the mobile robot motion control system through inverse transformation.

2. The mobile robot trajectory tracking control method based on high-order full-drive theory according to claim 1 is characterized in that: The step S4 comprises: Set the prediction interval N=K, and the recursive expression is as follows: (10) in Respectively exist Recursion of time; Defining intermediate variables ,in in " indicates omitted , then equation (10) can be written in matrix form: (11) in Respectively of Power; Defining quadratic performance indicators for: (12) in, , Respectively The transpose of represents the state weight matrix in diagonal form, represents the control weight matrix in diagonal form; Quadratic performance indicators through Yalmip toolbox Solve and obtain the optimal control sequence ; Take the first element of the sequence As the system control law at the current moment, and according to and , the MPC controller obtains The inverse transformation is ,according to The corresponding control quantity is obtained to realize the trajectory tracking predictive control of the mobile robot.

3. A mobile robot trajectory tracking prediction control device based on high-order full-drive theory, characterized in that: It includes one or more processors for implementing a mobile robot trajectory tracking control method based on high-order full-drive theory as described in claim 1 or 2.

4. A readable storage medium, characterized in that: A program is stored thereon, and when the program is executed by a processor, a mobile robot trajectory tracking control method based on high-order full-drive theory as described in claim 1 or 2 is implemented.

Images