WO2022000967A1

WO2022000967A1 - Path tracking control method and system, and computer readable storage medium

Info

Publication number: WO2022000967A1
Application number: PCT/CN2020/131789
Authority: WO
Inventors: 刘胜明; 杨松岩; 司秀芬
Original assignee: 苏州艾吉威机器人有限公司
Priority date: 2020-07-02
Filing date: 2020-11-26
Publication date: 2022-01-06
Also published as: CN113885317A

Abstract

A path tracking control method and system, and a computer readable storage medium. The path tracking control method comprises the following steps: establishing a kinematic model and a state space equation for a mobile robot (S101); calculating a matrix coefficient by means of a system state variable and a control input (S102); establishing a Cost-Function objective function, and obtaining a state feedback controller in the case that the objective function is minimized within a prediction period (S103); and obtaining an optimal control input within the next prediction period and executing the step S103 (S104). According to the path tracking control method and system and the computer readable storage medium, the mobile robot can be accurately controlled.

Description

A path tracking control method, system and computer readable storage medium

technical field

The present invention relates to the technical field, and in particular, to a path tracking control method, system and computer-readable storage medium.

Background technique

Path tracking is a key technology in the research direction of unmanned vehicles. The path tracking control method refers to a control method that can make the unmanned vehicle drive safely and stably according to the preset path.

Proportional-integral-derivative control, referred to as PID control, is one of the earliest developed control strategies. Because of its simple algorithm, good robustness and high reliability, it is widely used in industrial process control, and there are still about 90% of the control loops. Has a PID structure.

Simply put, the control deviation is formed according to the given value and the actual output value, and the deviation is formed by linear combination of proportional, integral and differential to form the control amount, and the controlled object is controlled. A conventional PID controller acts as a linear controller.

In the control of mobile robots, PID control is not accurate enough.

SUMMARY OF THE INVENTION

In view of this, the technical problem to be solved by the present invention is to provide a path tracking control method, system and computer-readable storage medium, which can precisely control a mobile robot.

The technical scheme of the present invention is realized as follows:

A path tracking control method, comprising the following steps:

S1. Establish the kinematics model and state space equation of the mobile robot;

S2. Calculate matrix coefficients from system state variables and control inputs;

S3. Establish a Cost-Function objective function to obtain a state feedback controller that minimizes the objective function within the prediction period;

S4. Obtain the optimal control input in the next prediction period and execute S3.

Preferably, in the S1, the state equation of the controlled system is:

u(t)=K(t)x(t)

where x(t) represents the real-time pose of the robot, which can be represented by coordinates and angles as

u(t) represents the control amount of the robot, which is represented by the speed and angle of the steering wheel

K(t) is the state feedback controller;

Then the control state equation of the system is expressed as:

The input volume is

The observed value is

Preferably, the steering wheel speed V _{wheel of} the mobile robot, the steering wheel angle α _wheel , the distance L from the motion center to the steering wheel; the angular velocity of the vehicle body: w=V _wheel *sin(α _wheel )/L

Vehicle body motion center speed: V _car =w*L/tanθ=V _wheel *cos(α _wheel )

but:

get

Linearize nonlinear systems.

will

respectively

and

Perform a first-order Taylor expansion to get the matrix coefficients A and B.

Establish a Cost-Function objective function to find the state feedback controller K(t) under the condition that the objective function is minimized within the prediction period; establish a Cost-Function objective function to find an optimal control function u(t), so that the system can change from given When the given state x ₀ starts and transfers to the target control final state x _tf , the system index is optimized

Among them, Q(t) and R(t) are the control weights of x(t) and u(t) respectively, Q(t) is a semi-positive definite matrix, and R(t) is a positive definite matrix. Substituting (2) into (6), we get

According to the Riccati method

K(t)=R ^-1 (t)B ^T (t)P(t) (8)

A ^T (t)P(t)+P(t)A(t)+Q(t)-P(t)B(t)R ^-1 (t)B ^T (t)P(t)=0 ( 9)

The value of K(t) is related to P(t), and P(t) is an assumed quantity, and P(t) only needs to make equation (9) true. After P(t) is obtained, the feedback matrix K(t) that minimizes the Cost-Function objective function can be obtained according to equation (8). Obtain the optimal control input u(t) in the next prediction cycle. According to the formula (2) u(t)=K(t) x(t), the final control quantity can be obtained, that is, the speed of the steering wheel in the next cycle and angle, go to step 3.

The present invention also proposes a path tracking control system, including:

Establishment unit for establishing the kinematic model and state space equation of the mobile robot;

a calculation unit for calculating matrix coefficients from system state variables and control inputs;

The acquisition unit is used to establish the Cost-Function objective function, and acquire the state feedback controller under the condition that the objective function is minimized in the prediction period;

The execution unit is used for obtaining the optimal control input in the next prediction period and starting the obtaining unit.

The present invention also provides a path tracking control device, including a memory and a processor; the memory is used to store a computer program; the processor is used to implement the above-mentioned path tracking control method when the computer program is executed .

The present invention also provides a computer-readable storage medium, characterized in that, a computer program is stored on the storage medium, and when the computer program is executed by the processor, the above-mentioned path tracking control method is implemented.

Compared with the traditional PID control, the path tracking control method, system and computer-readable storage medium proposed by the present invention adopts the idea of LQR optimal control, which makes the control more accurate.

Description of drawings

1 is a flowchart of a path tracking control method proposed by an embodiment of the present invention;

2 is a structural block diagram of a path tracking control system proposed by an embodiment of the present invention;

FIG. 3 is a vehicle model diagram of a path tracking control method proposed by an embodiment of the present invention.

detailed description

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, but not all of 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.

As shown in FIG. 1 , an embodiment of the present invention provides a path tracking control method, which includes the following steps:

S101. Establish a kinematic model and state space equation of the mobile robot;

S102, calculating matrix coefficients from system state variables and control inputs;

S103, establishing a Cost-Function objective function, and obtaining a state feedback controller under the condition that the objective function is minimized within the prediction period;

S104, obtain the optimal control input in the next prediction period and execute S103.

It can be seen that, compared with the traditional PID control, the path tracking control method, system and computer-readable storage medium proposed in the embodiments of the present invention adopt the idea of LQR optimal control, which makes the control more accurate.

The specific steps of the embodiment of the present invention are as follows:

Establish the kinematic model of the mobile robot and the state space equation;

The state equation of the known controlled system is:

u(t)=K(t)x(t) (2)

K(t) is the state feedback controller.

Then the control state equation of the system can be expressed as:

The input volume is

The observed value is

From the system state variables and control input, find the matrix coefficients A(t) and B(t)

The known vehicle model is shown in Figure 3; the steering wheel speed V _{wheel of the} mobile robot, the steering wheel angle α _wheel , the distance L from the motion center to the steering wheel; the angular velocity of the vehicle body: w=V _wheel *sin(α _wheel )/L

Vehicle body motion center speed: V _car =w*L/tanθ=V _wheel *cos(α _wheel ):

Substitute (4) into (1) to get:

Linearization is required for nonlinear systems.

will

respectively

and

The first-order Taylor expansion can be used to obtain the matrix coefficients A and B in equation (1).

Establish a Cost-Function objective function, and find the state feedback controller K(t) under the condition that the objective function is minimized in the prediction period;

Establish a Cost-Function objective function and find an optimal control function u(t), so that when the system transitions from a given state x ₀ to the target control final state x _tf , the system index is optimal

According to the Riccati method:

K(t)=R ^-1 (t)B ^T (t)P(t) (8)

A ^T (t)P(t)+P(t)A(t)+Q(t)-P(t)B(t)R ^-1 (t)B ^T (t)P(t)=0 ( 9)

As shown in Figure 2, the present invention also proposes a path tracking control system, including:

establishing unit 21 for establishing a kinematic model and state space equation of the mobile robot;

The calculation unit 22 is used for calculating matrix coefficients from system state variables and control inputs;

The acquisition unit 23 is used to establish the Cost-Function objective function, and acquires the state feedback controller under the condition that the objective function is minimized in the prediction period;

The execution unit 24 is configured to obtain the optimal control input in the next prediction period and start the obtaining unit.

An embodiment of the present invention also provides a path tracking control device, including a memory and a processor; the memory is used to store a computer program; the processor is used to implement the above path tracking control when the computer program is executed method.

The embodiments of the present invention also provide a computer-readable storage medium, where a computer program is stored on the storage medium, and when the computer program is executed by the processor, the above-mentioned path tracking control method is implemented.

From the description of the above embodiments, those skilled in the art can clearly understand that the present application can be implemented by means of software plus necessary general-purpose hardware. Special components, etc. to achieve. Under normal circumstances, all functions completed by a computer program can be easily implemented by corresponding hardware, and the specific hardware structures used to implement the same function can also be various, such as analog circuits, digital circuits or special circuit, etc. However, a software program implementation is a better implementation in many cases for this application. Based on this understanding, the technical solutions of the present application can be embodied in the form of software products in essence, or the parts that make contributions to the prior art. The computer software products are stored in a readable storage medium, such as a floppy disk of a computer. , U disk, mobile hard disk, ROM, RAM, magnetic disk or optical disk, etc., including several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods of various embodiments of the present application.

In the above-mentioned embodiments, it may be implemented in whole or in part by software, hardware, firmware or any combination thereof. When implemented in software, it can be implemented in whole or in part in the form of a computer program product. A computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, the procedures or functions according to the embodiments of the present application are generated in whole or in part. The computer may be a general purpose computer, a special purpose computer, a computer network, or other programmable device. Computer instructions may be stored in or transmitted from one computer-readable storage medium to another computer-readable storage medium, for example, the computer instructions may be transmitted from a website site, computer, server, or data center over a wire (e.g. coaxial cable, fiber optic, digital subscriber line (DSL)) or wireless (eg, infrared, wireless, microwave, etc.) to another website site, computer, server, or data center. The computer-readable storage medium can be any available medium that can be stored by a computer or a data storage device such as a server, a data center, etc. that includes one or more available media integrated. Useful media may be magnetic media (eg, floppy disks, hard disks, magnetic tapes), optical media (eg, DVDs), or semiconductor media (eg, solid state disks (SSDs)), and the like.

Finally, it should be noted that the above descriptions are only preferred embodiments of the present invention, and are only used to illustrate the technical solutions of the present invention, but not to limit the protection scope of the present invention. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention are included in the protection scope of the present invention.

Claims

A path tracking control method, comprising the following steps:

S1. Establish the kinematic model and state space equation of the mobile robot;

S2. Calculate matrix coefficients from system state variables and control inputs;

S3. Establish a Cost-Function objective function, and obtain a state feedback controller that minimizes the objective function within the prediction period;

S4. Obtain the optimal control input in the next prediction period and execute S3.
The path tracking control method according to claim 1, wherein in the S1, the state equation of the controlled system is:

u(t)=K(t)x(t)

where x(t) represents the real-time pose of the robot, which can be represented by coordinates and angles as

u(t) represents the control amount of the robot, which is represented by the speed and angle of the steering wheel

K(t) is the state feedback controller;

Then the control state equation of the system is expressed as:

The input volume is
The observed value is
The path tracking control method according to claim 2, wherein, the steering wheel speed v wheel of the mobile robot, the steering wheel angle α wheel , the distance L from the motion center to the steering wheel; the vehicle body angular velocity: w=V wheel *sin(α wheel )/L

Vehicle body motion center speed: V car +w*L/tanθ=v wheel *cos(α wheel )

but:

get

Linearize nonlinear systems; put
respectively
and
Perform a first-order Taylor expansion to get the matrix coefficients A and B.
The path tracking control method according to claim 2, wherein the S3 specifically comprises:

Establish a Cost-Function objective function and find an optimal control function u(t), so that when the system transitions from a given state x 0 to the target control final state x tf , the system index is optimal;

Among them, Q(t) and R(t) are the control weights of x(t) and u(t) respectively, Q(t) is a semi-positive definite matrix, and R(t) is a positive definite matrix;

Calculated:

According to the Riccati method:

K(t)=R -1 (t)B T (t)P(t)

A T (t)P(t)+P(t)A(t)+Q(t)-P(t)B(t)R -1 (t)B T (t)P(t)=0

Compute the feedback matrix K(t) that minimizes the Cost-Function objective function.
A path tracking control system, characterized in that it includes:

Establishment unit for establishing the kinematic model and state space equation of the mobile robot;

a calculation unit for calculating matrix coefficients from system state variables and control inputs;

The acquisition unit is used to establish the Cost-Function objective function, and acquire the state feedback controller under the condition that the objective function is minimized in the prediction period;

The execution unit is used for obtaining the optimal control input in the next prediction period and starting the obtaining unit.
A path tracking control device, characterized in that it includes a memory and a processor; the memory is used to store a computer program; the processor is used to implement any one of claims 1-4 when executing the computer program The described path tracking control method.
A computer-readable storage medium, characterized in that a computer program is stored on the storage medium, and when the computer program is executed by a processor, the path tracking control method according to any one of claims 1-4 is implemented.