Mobile robot trajectory tracking control method and device based on high-order all-drive theory and medium

Abstract

The invention belongs to the field of robot motion control, and discloses a mobile robot trajectory tracking control method and device based on a high-order all-drive theory, and a medium, and the method comprises the steps: constructing a mobile robot two-wheel bicycle model, and converting a state space model into an all-drive model; aiming at the trajectory tracking control problem of the mobile robot two-wheeled bicycle model, designing an all-drive stabilization controller to obtain a mobile robot trajectory tracking control system error model; according to a mobile robot trajectory tracking control system error model, a system increment equation is introduced to simplify a mathematical model; and an MPC controller is designed to solve the optimal control sequence, and the final input of the mobile robot motion control system is obtained through inverse transformation. According to the method, a complex state space model is converted into a more concise second-order all-drive model by using a high-order all-drive theory, so that the physical significance of a system is reserved, the problem of an ill-conditioned matrix possibly encountered in a traditional method is avoided, and the design process of the controller is greatly simplified.

Description

Technical Field

[0001] The present invention relates to the field of robot motion control, and in particular to a mobile robot trajectory tracking control method, device and medium based on high-order full-drive theory. Background Art

[0002] With the rapid development of the robotics industry and the rapid growth of its related modules, various robots have been widely used in military and civilian fields. As an important branch of the robotics industry, the performance and technical level of mobile robots have also made significant progress. When performing tasks such as autonomous inspections and fixed-point supply of materials, these 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 refers to the process of a mobile robot starting from a specific initial state and accurately following a reference trajectory that changes over time. The design of a path tracking controller is challenging due to factors such as model uncertainty, sensor noise, and external disturbances. To this end, technologies such as reinforcement learning, sliding mode control, robust control, and model predictive control (MPC) have been proposed to address the trajectory tracking problem of mobile robots. Among them, MPC is based on the characteristic modeling of the controlled object, makes decisions by predicting future state change trends, and has good feedback correction capabilities and robustness, making it an effective method to solve the trajectory tracking problem of mobile robots.

[0004] It is worth noting 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 emergence of ill-conditioned matrices during the model simplification process. In recent years, the theory of all-wheel drive control systems 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 ill-conditioned matrix problem that may occur during the simplification process, so it can more accurately represent the actual system. In addition, it provides a simpler method for the design of the controller, which helps to improve the trajectory tracking accuracy and efficiency of the mobile robot. Summary of the invention

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

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

[0007] A mobile robot trajectory tracking control method based on high-order full-drive theory, comprising:

[0008] Step S1, constructing a mobile robot two-wheel bicycle model, and converting the state space model into an all-wheel drive model;

[0009] 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;

[0010] Step S3, according to the mobile robot trajectory tracking control system error model given in step S2, introducing 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 comprises:

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

[0014] (1)

[0015] 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;

[0016] right Take the second derivative:

[0017] (2)

[0018] in Respectively The second-order derivative of , design control quantity , substituting into equation (2) yields the following state space equation:

[0019] (3)

[0020] in express The second-order derivative of After discretization, the discrete state space equation is obtained as follows:

[0021] (4)

[0022] 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.

[0023] Furthermore, the step S2 comprises:

[0024] Design the all-wheel drive stabilization controller as follows:

[0025] (5)

[0026] 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;

[0027] Substituting equation (4) into equation (3), the closed-loop high-order all-wheel drive tracking error control model is obtained as follows:

[0028] (6)

[0029] in is an identity matrix of appropriate order.

[0030] Furthermore, the step S3 comprises:

[0031] Remember the intermediate variables , , then the high-order all-wheel drive tracking error control model shown in equation (6) can be re-expressed as:

[0032] (7)

[0033] in, is the system parameter matrix, where is the identity matrix of suitable order;

[0034] Remember the intermediate variables , , , the incremental equation of formula (7) can be obtained as follows:

[0035] (8)

[0036] Remember the intermediate variables , ,have:

[0037] (9)

[0038] in .

[0039] Furthermore, 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.

[0040] The present invention also provides a mobile robot trajectory tracking prediction control device based on high-order all-wheel drive theory, including one or more processors for implementing the mobile robot trajectory tracking control method based on high-order all-wheel drive theory as described above.

[0041] The present invention also provides a readable storage medium having a program stored thereon, and when the program is executed by a processor, the mobile robot trajectory tracking control method based on high-order full-drive theory as described above is implemented.

[0042] Compared with the prior art, the present invention has the following beneficial effects:

[0043] 1) The complex state-space model is converted into a simpler second-order all-wheel drive model using high-order all-wheel drive theory, 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, thereby greatly simplifying the controller design process and improving design efficiency.

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

[0045] 3) Introducing system incremental equations 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.

[0046] 4) An MPC method based on a high-order all-wheel drive model is proposed to solve the optimal control laws. Through the corresponding inverse transformation relationships, these control laws are converted back into control instructions suitable for the all-wheel drive system, and finally the trajectory tracking predictive control of the mobile robot based on high-order all-wheel drive theory is realized. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 This is a flow chart of a mobile robot trajectory tracking control method based on high-order full-drive theory of the present invention.

[0048] Figure 2 This is a comparison curve diagram of a reference trajectory and an actual motion trajectory of a robot in a trajectory tracking control method of a mobile robot based on high-order full-drive theory of the present invention.

[0049] Figure 3 It is a structural schematic diagram of a mobile robot trajectory tracking prediction control device based on high-order full-drive theory of the present invention. DETAILED DESCRIPTION

[0050] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0051] In one embodiment of the present application, a mobile robot trajectory tracking control method based on high-order full-drive theory is proposed, comprising the following steps:

[0052] Step S1: construct a mobile robot two-wheel bicycle model, design an all-wheel drive control law, and convert the state space model into an all-wheel drive model:

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

[0054] (1)

[0055] 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, .

[0056] right Take the second derivative:

[0057] (2)

[0058] in Respectively The second-order derivative of , design control quantity , substituting into equation (2) yields the following state space equation:

[0059] (3)

[0060] in express The second-order derivative of To discretize, , the discrete state space equation is as follows:

[0061] (4)

[0062] in, Indicates The sampling time, Respectively represent Sampling time The value of Respectively The coefficient of It indicates the all-wheel drive stabilization controller that needs to be designed. Among them, , .

[0063] Step S2: for the mobile robot two-wheel bicycle model given by equation (3), an all-wheel drive stabilization controller is designed for its trajectory tracking control problem, and an error model of the mobile robot trajectory tracking control system under the high-order all-wheel drive model is obtained. The all-wheel drive stabilization controller is as follows:

[0064] (5)

[0065] 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. The poles of the closed-loop system are configured as , to obtain .

[0066] Substituting equation (4) into equation (3), the closed-loop high-order all-wheel drive tracking error control model is obtained as follows:

[0067] (6)

[0068] in is an identity matrix of appropriate order.

[0069] 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:

[0070] Remember the intermediate variables , , then the high-order all-wheel drive tracking error control model shown in equation (6) can be re-expressed as:

[0071] (7)

[0072] in, is the system parameter matrix, where is an identity matrix of appropriate order. .

[0073] Remember the intermediate variables , , , the incremental equation of formula (7) can be obtained as follows: (8)

[0074] Remember the intermediate variables , ,have:

[0075] (9)

[0076] in , as follows:

[0077] .

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

[0079] Setting prediction intervals , N can be other numbers, and the recursive expression is as follows:

[0080] (10)

[0081] in Respectively exist The recursion of time.

[0082] Defining intermediate variables ,in in " indicates omitted and , then equation (10) can be written in matrix form:

[0083] (11)

[0084] in Respectively 2, 3, 4, 5 powers, and

[0085]

[0086]

[0087]

[0088]

[0089]

[0090] Defining quadratic performance indicators for:

[0091] (12)

[0092] in, , Respectively The transpose of , represents the state weight matrix in diagonal form, , Represents the control weight matrix in diagonal form.

[0093] 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.

[0094] See also Figure 3 An embodiment of the present invention provides a mobile robot trajectory tracking prediction control device based on high-order full-wheel drive theory, including one or more processors, which are used to implement a mobile robot trajectory tracking control method based on high-order full-wheel drive theory in the above embodiment.

[0095] An embodiment of a mobile robot trajectory tracking prediction control device based on high-order full-wheel drive theory of the present invention can be applied to any device with data processing capabilities, and the device with data processing capabilities can be a device or apparatus such as a computer. The device embodiment can be implemented through software, or through hardware or a combination of software and hardware. Taking software implementation as an example, as a device in a logical sense, it is formed by the processor of any device with data processing capabilities in which it is located reading the corresponding computer program instructions in the non-volatile memory into the internal memory for execution. From a hardware perspective, if Figure 3 As shown in FIG. 1 , a hardware structure diagram of a mobile robot trajectory tracking prediction control device based on high-order full-drive theory of the present invention is located in any device with data processing capability, except Figure 3In addition to the processor, memory, network interface, and non-volatile memory shown, any device with data processing capabilities in the embodiments may also include other hardware, usually based on the actual functions of the device with data processing capabilities, which will not be described in detail.

[0096] The implementation process of the functions and effects of each unit in the above-mentioned device is specifically described in the implementation process of the corresponding steps in the above-mentioned method, and will not be repeated here.

[0097] 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.

[0098] An embodiment of the present invention further provides a readable storage medium having a program stored thereon. When the program is executed by a processor, a mobile robot trajectory tracking control method based on high-order full-drive theory in the above embodiment is implemented.

[0099] The readable storage medium may be an internal storage unit of any device with data processing capability described in any of the aforementioned embodiments, such as a hard disk or a 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. Furthermore, the readable storage medium may also include both an internal storage unit of any device with data processing capability and an 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 capability, and may also be used to temporarily store data that has been output or is to be output.

[0100] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that 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, constructing a mobile robot two-wheel bicycle model, and converting the state space model into an all-wheel drive model; 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; Step S3, according to the mobile robot trajectory tracking control system error model given in step S2, introducing the system increment equation to simplify the mathematical model; 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 S1 comprises: 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-order 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.

3. The mobile robot trajectory tracking control method based on high-order full-drive theory according to claim 2 is characterized in that: The step S2 comprises: 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 an identity matrix of suitable order.

4. The mobile robot trajectory tracking control method based on high-order full-drive theory according to claim 2 is characterized in that: The step S3 comprises: 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 .

5. The mobile robot trajectory tracking control method based on high-order full-drive theory according to claim 2 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.

6. 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 any one of claims 1-5.

7. 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 any one of claims 1-5 is implemented.

Images