CN113885317A - Path tracking control method, system and computer readable storage medium - Google Patents
Path tracking control method, system and computer readable storage medium Download PDFInfo
- Publication number
- CN113885317A CN113885317A CN202010637988.8A CN202010637988A CN113885317A CN 113885317 A CN113885317 A CN 113885317A CN 202010637988 A CN202010637988 A CN 202010637988A CN 113885317 A CN113885317 A CN 113885317A
- Authority
- CN
- China
- Prior art keywords
- function
- control
- state
- control method
- path tracking
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Feedback Control In General (AREA)
Abstract
The invention provides a path tracking control method, a system and a computer readable storage medium, wherein the path tracking control method is characterized by comprising the following steps: s1, establishing a kinematic model and a state space equation of the mobile robot; s2, calculating matrix coefficients according to the system state variables and control inputs; s3, establishing a Cost-Function target Function, and acquiring a state feedback controller under the condition that the target Function is minimum in a prediction period; s4, obtaining the optimal control input in the next prediction period and executing S3. The path tracking control method, the path tracking control system and the computer readable storage medium can accurately control the mobile robot.
Description
Technical Field
The present invention relates to the field of technologies, and in particular, to a method and a system for path tracking control and a computer-readable storage medium.
Background
Path tracking is a key technology in the direction of unmanned vehicle research. The path tracking control method is a control method which can enable the unmanned vehicle to safely and stably run according to a preset path.
Proportional-integral-derivative control, PID control for short, is one of the earliest developed control strategies, and because of its simple algorithm, good robustness and high reliability, it is widely used in industrial process control, and so far, about 90% of control loops have PID structures.
In brief, a control deviation is formed according to a given value and an actual output value, the deviation is combined in proportion, integral and differential through linearity to form a control quantity, and a controlled object is controlled. A conventional PID controller acts as a linear controller.
When the mobile robot is controlled, the PID control is not accurate enough.
Disclosure of Invention
In view of the above, the technical problem to be solved by the present invention is to provide a method, a system and a computer readable storage medium for path tracking control, which can perform precise control on a mobile robot.
The technical scheme of the invention is realized as follows:
a path tracking control method, comprising the steps of:
s1, establishing a kinematic model and a state space equation of the mobile robot;
s2, calculating matrix coefficients according to the system state variables and control inputs;
s3, establishing a Cost-Function target Function, and acquiring a state feedback controller under the condition that the target Function is minimum in a prediction period;
s4, obtaining the optimal control input in the next prediction period and executing S3.
Preferably, in S1, the state equation of the controlled system is:
u(t)=K(t)x(t)
wherein x (t) represents the real-time pose of the robot, which can be represented by coordinates and angles
u (t) represents the control amount of the robot, which is represented by the speed and angle of the steering wheel
K (t) is a state feedback controller;
the control state equation for the system is then expressed as:
Preferably, the mobile robot steering wheel speed VwheelSteering wheel angle alphawheelThe distance L from the motion center to the steering wheel; angular velocity of vehicle body: w ═ Vwheel*sin(αwheel)/L
Vehicle body movement center speed: vcar=w*L/tanθ=Vwheel*cos(αwheel)
Then:
to obtain
Linearization is performed for a nonlinear system.
Will be provided withAre respectively pairedAndand performing first-order Taylor expansion to obtain matrix coefficients A and B.
Establishing a Cost-Function target Function, and solving a state feedback controller K (t) under the condition that the target Function is minimum in a prediction period; establishing a Cost-Function objective Function, and finding an optimal control Function u (t) to enable the system to be in a given state x0Transition from departure to target control end state xtfMake the system index optimal
Wherein Q (t), R (t) are the control weights of x (t), u (t), respectively, Q (t) is a semi-positive definite matrix, and R (t) is a positive definite matrix. Substituting the formula (2) into the formula (6) to obtain
According to the Riccati method
K(t)=R-1(t)BT(t)P(t) (8)
AT(t)P(t)+P(t)A(t)+Q(t)-P(t)B(t)R-1(t)BT(t)P(t)=0 (9)
The value of K (t) is related to P (t), which is an assumed quantity, and P (t) is only required to satisfy equation (9).
After P (t) is obtained, the feedback matrix K (t) for minimizing the Cost-Function objective Function can be obtained according to the formula (8). The optimal control input u (t) in the next prediction cycle is obtained by obtaining the final control amount, that is, the speed and angle of the steering wheel in the next cycle, based on the formula (2) u (t) ═ k (t) · x (t), and the procedure goes to step 3.
The invention also provides a path tracking control system, comprising:
the building unit is used for building a kinematic model and a 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 for establishing a Cost-Function target Function and acquiring the state feedback controller under the condition that the target Function is minimum in the prediction period;
and the execution unit is used for obtaining the optimal control input in the next prediction period and starting the acquisition unit.
The invention also provides a path tracking control device, which comprises a memory and a processor; the memory for storing a computer program; the processor is configured to implement the path tracking control method when executing the computer program.
The present invention also provides a computer-readable storage medium, wherein the storage medium stores a computer program, and when the computer program is executed by a processor, the computer program implements the path tracking control method.
Compared with the traditional PID control, the path tracking control method, the system and the computer readable storage medium provided by the invention adopt the idea of the optimal LQR control, so that the control is more accurate.
Drawings
Fig. 1 is a flowchart of a path tracking control method according to an embodiment of the present invention;
fig. 2 is a block diagram of a path tracking control system according to an embodiment of the present invention;
fig. 3 is a vehicle model diagram of a path tracking control method according to 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 drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, 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, including the following steps:
s101, establishing a kinematic model and a state space equation of the mobile robot;
s102, calculating matrix coefficients according to system state variables and control input;
s103, establishing a Cost-Function target Function, and acquiring a state feedback controller under the condition that the target Function is minimum in a prediction period;
and S104, obtaining the optimal control input in the next prediction period and executing S103.
Therefore, compared with the traditional PID control, the path tracking control method, the system and the computer readable storage medium provided by the embodiment of the invention adopt the idea of the optimal LQR control, so that the control is more accurate.
The embodiment of the invention comprises the following specific steps:
establishing a kinematic model of the mobile robot and a state space equation;
the state equation of the known controlled system is:
u(t)=K(t)x(t) (2)
wherein x (t) represents the real-time pose of the robot, which can be represented by coordinates and angles
u (t) represents the control amount of the robot, which is represented by the speed and angle of the steering wheel
K (t) is a state feedback controller.
The control state equation for the system can be expressed as:
Calculating matrix coefficients A (t) and B (t) from system state variables and control inputs
A known vehicle model is shown in FIG. 3; speed V of steering wheel of mobile robotwheelSteering wheel angle alphawheelThe distance L from the motion center to the steering wheel; angular velocity of vehicle body: w ═ Vwheel*sin(αwheel)/L
Vehicle body movement center speed: vcar=w*L/tanθ=Vwheel*cos(αwheel)
Then:
substituting the formula (4) into the formula (1) to obtain:
linearization of non-linear systems is required.
Will be provided withAre respectively pairedAndmatrix coefficients a and B in equation (1) can be obtained by performing first-order taylor expansion.
Establishing a Cost-Function target Function, and solving a state feedback controller K (t) under the condition of minimizing the target Function in a prediction period
Establishing a Cost-Function objective Function, and finding an optimal control Function u (t) to enable the system to be in a given state x0Transition from departure to target control end state xtfMake the system index optimal
Wherein Q (t), R (t) are the control weights of x (t), u (t), respectively, Q (t) is a semi-positive definite matrix, and R (t) is a positive definite matrix. Substituting the formula (2) into the formula (6) to obtain
According to the Riccati method:
K(t)=R-1(t)BT(t)P(t) (8)
AT(t)P(t)+P(t)A(t)+Q(t)-P(t)B(t)R-1(t)BT(t)P(t)=0 (9)
the value of K (t) is related to P (t), which is an assumed quantity, and P (t) is only required to satisfy equation (9). After P (t) is obtained, the feedback matrix K (t) for minimizing the Cost-Function objective Function can be obtained according to the formula (8). The optimal control input u (t) in the next prediction cycle is obtained by obtaining the final control amount, that is, the speed and angle of the steering wheel in the next cycle, based on the formula (2) u (t) ═ k (t) · x (t), and the procedure goes to step 3.
As shown in fig. 2, the present invention further provides a path tracking control system, including:
the establishing unit 21 is used for establishing a kinematic model and a state space equation of the mobile robot;
a calculation unit 22 for calculating matrix coefficients from system state variables and control inputs;
an obtaining unit 23, configured to establish a Cost-Function target Function, and obtain a state feedback controller under a condition that the target Function is minimized within a prediction period;
and the execution unit 24 is used for obtaining the optimal control input in the next prediction period and starting the acquisition unit.
The embodiment of the invention also provides a path tracking control device, which comprises a memory and a processor; the memory for storing a computer program; the processor is configured to implement the above-mentioned path tracking control method when executing the computer program.
An embodiment of the present invention further provides a computer-readable storage medium, where a computer program is stored on the storage medium, and when the computer program is executed by a processor, the path tracking control method is implemented.
Through the above description of the embodiments, those skilled in the art will clearly understand that the present application can be implemented by software plus necessary general-purpose hardware, and certainly can also be implemented by special-purpose hardware including special-purpose integrated circuits, special-purpose CPUs, special-purpose memories, special-purpose components and the like. Generally, functions performed by computer programs can be easily implemented by corresponding hardware, and specific hardware structures for implementing the same functions may be various, such as analog circuits, digital circuits, or dedicated circuits. However, for the present application, the implementation of a software program is more preferable. Based on such understanding, the technical solutions of the present application may be substantially embodied in the form of a software product, which is stored in a readable storage medium, such as a floppy disk, a usb disk, a removable hard disk, a ROM, a RAM, a magnetic disk, or an optical disk of a computer, and includes several instructions for enabling a computer device (which may be a personal computer, a server, or a network device) to execute the method of the embodiments of the present application.
In the above embodiments, the implementation may be wholly or partially realized by software, hardware, firmware, or any combination thereof. When implemented in software, may be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions. The procedures or functions according to the embodiments of the present application are all or partially generated when the computer program instructions are loaded and executed on a computer. The computer may be a general purpose computer, a special purpose computer, a network of computers, or other programmable device. The computer instructions may be stored in a computer readable storage medium or transmitted from one computer readable storage medium to another, e.g., the computer instructions may be transmitted from one website, computer, server, or data center to another website, computer, server, or data center via wired (e.g., coaxial cable, fiber optic, Digital Subscriber Line (DSL)) or wireless (e.g., infrared, wireless, microwave, etc.) means. A computer-readable storage medium may be any available medium that a computer can store or a data storage device, such as a server, a data center, etc., that is integrated with one or more available media. The usable medium may be a magnetic medium (e.g., floppy disk, hard disk, magnetic tape), an optical medium (e.g., DVD), or a semiconductor medium (e.g., Solid State Disk (SSD)), among others.
Finally, it is to be noted that: the above description is only a preferred embodiment of the present invention, and is only used to illustrate the technical solutions of the present invention, and not to limit the protection scope of the present invention. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention shall fall within the protection scope of the present invention.
Claims (7)
1. A path tracking control method, comprising the steps of:
s1, establishing a kinematic model and a state space equation of the mobile robot;
s2, calculating matrix coefficients according to the system state variables and control inputs;
s3, establishing a Cost-Function target Function, and acquiring a state feedback controller under the condition that the target Function is minimum in a prediction period;
s4, obtaining the optimal control input in the next prediction period and executing S3.
2. The path-tracing control method according to claim 1, wherein in S1, the state equation of the controlled system is:
u(t)=K(t)x(t)
wherein x (t) represents the real-time pose of the robot, which can be represented by coordinates and angles
u (t) represents the control amount of the robot, which is represented by the speed and angle of the steering wheel
K (t) is a state feedback controller;
the control state equation for the system is then expressed as:
3. The path-tracing control method of claim 2, whereinSpeed V of steering wheel of mobile robotwheelSteering wheel angle alphawheelThe distance L from the motion center to the steering wheel; angular velocity of vehicle body: w ═ Vwheel*sin[αwheel)/L
Vehicle body movement center speed: vcar=w*L/tanθ=Vwheel*cos(αwheel)
Then:
to obtain
Linearizing the nonlinear system;
4. The path tracking control method according to claim 2, wherein the S3 specifically includes:
establishingThe Cost-Function objective Function finds an optimal control Function u (t) to make the system from a given state x0Transition from departure to target control end state xtfThe system index is optimized;
wherein Q (t), R (t) are the control weights of x (t), u (t), respectively, Q (t) is a semi-positive definite matrix, R (t) is a positive definite matrix;
and calculating to obtain:
according to the Riccati method:
K(t)=R-1(t)BT(t)P(t)
AT(t)P(t)+P(t)A(t)+Q(t)-P(t)B(t)R-1(t)BT(t)P(t)=0
the feedback matrix k (t) that minimizes the Cost-Function objective Function is calculated.
5. A path tracking control system, comprising:
the building unit is used for building a kinematic model and a 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 for establishing a Cost-Function target Function and acquiring the state feedback controller under the condition that the target Function is minimum in the prediction period;
and the execution unit is used for obtaining the optimal control input in the next prediction period and starting the acquisition unit.
6. A path tracking control apparatus, comprising a memory and a processor; the memory for storing a computer program; the processor, when executing the computer program, is configured to implement the path-tracing control method according to any of claims 1-4.
7. Computer-readable storage medium, characterized in that the storage medium has stored thereon a computer program which, when being executed by a processor, carries out a path-tracing control method according to any one of claims 1-4.
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010637988.8A CN113885317A (en) | 2020-07-02 | 2020-07-02 | Path tracking control method, system and computer readable storage medium |
PCT/CN2020/131789 WO2022000967A1 (en) | 2020-07-02 | 2020-11-26 | Path tracking control method and system, and computer readable storage medium |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010637988.8A CN113885317A (en) | 2020-07-02 | 2020-07-02 | Path tracking control method, system and computer readable storage medium |
Publications (1)
Publication Number | Publication Date |
---|---|
CN113885317A true CN113885317A (en) | 2022-01-04 |
Family
ID=79013365
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010637988.8A Pending CN113885317A (en) | 2020-07-02 | 2020-07-02 | Path tracking control method, system and computer readable storage medium |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN113885317A (en) |
WO (1) | WO2022000967A1 (en) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115077549B (en) * | 2022-06-16 | 2024-04-26 | 南昌智能新能源汽车研究院 | Vehicle state tracking method, system, computer and readable storage medium |
CN117991648B (en) * | 2024-04-07 | 2024-06-07 | 山东科技大学 | AGV sliding mode path tracking control method based on disturbance compensation |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20160244077A1 (en) * | 2015-02-23 | 2016-08-25 | Mitsubishi Electric Research Laboratories, Inc. | System and Method for Stopping Trains Using Simultaneous Parameter Estimation |
CN109491389A (en) * | 2018-11-23 | 2019-03-19 | 河海大学常州校区 | A kind of robot trace tracking method with constraint of velocity |
CN110286683A (en) * | 2019-07-15 | 2019-09-27 | 北京科技大学 | A kind of autonomous running path tracking control method of caterpillar mobile robot |
CN110348115A (en) * | 2019-07-09 | 2019-10-18 | 顺丰科技有限公司 | A kind of LQR control method, device, storage medium and the controller of fork truck |
CN110850895A (en) * | 2019-08-05 | 2020-02-28 | 中国第一汽车股份有限公司 | Path tracking method, device, equipment and storage medium |
CN111273551A (en) * | 2020-03-12 | 2020-06-12 | 山东大学 | Double-steering wheel automatic guided vehicle trajectory tracking control method and system |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108536171B (en) * | 2018-03-21 | 2020-12-29 | 电子科技大学 | Path planning method for collaborative tracking of multiple unmanned aerial vehicles under multiple constraints |
CN108490780A (en) * | 2018-03-21 | 2018-09-04 | 中南大学 | A kind of quadrotor drone wide-angle complexity Attitude tracking control method of low-power consumption |
CN108919640B (en) * | 2018-04-20 | 2020-04-03 | 西北工业大学 | Method for realizing self-adaptive multi-target tracking of unmanned aerial vehicle |
CN110275549B (en) * | 2019-06-11 | 2022-03-11 | 深圳大学 | Multi-agent autonomous tracking method and device and computer-storable medium |
-
2020
- 2020-07-02 CN CN202010637988.8A patent/CN113885317A/en active Pending
- 2020-11-26 WO PCT/CN2020/131789 patent/WO2022000967A1/en active Application Filing
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20160244077A1 (en) * | 2015-02-23 | 2016-08-25 | Mitsubishi Electric Research Laboratories, Inc. | System and Method for Stopping Trains Using Simultaneous Parameter Estimation |
CN109491389A (en) * | 2018-11-23 | 2019-03-19 | 河海大学常州校区 | A kind of robot trace tracking method with constraint of velocity |
CN110348115A (en) * | 2019-07-09 | 2019-10-18 | 顺丰科技有限公司 | A kind of LQR control method, device, storage medium and the controller of fork truck |
CN110286683A (en) * | 2019-07-15 | 2019-09-27 | 北京科技大学 | A kind of autonomous running path tracking control method of caterpillar mobile robot |
CN110850895A (en) * | 2019-08-05 | 2020-02-28 | 中国第一汽车股份有限公司 | Path tracking method, device, equipment and storage medium |
CN111273551A (en) * | 2020-03-12 | 2020-06-12 | 山东大学 | Double-steering wheel automatic guided vehicle trajectory tracking control method and system |
Also Published As
Publication number | Publication date |
---|---|
WO2022000967A1 (en) | 2022-01-06 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Simonetto et al. | A class of prediction-correction methods for time-varying convex optimization | |
CN110221541B (en) | Feedforward coefficient acquisition device and method of feedforward controller in servo system | |
US20190176325A1 (en) | An Error Modeling Method For End-Effector Space-Curve Trajectory Of Six Degree-of-Freedom Robots | |
CN113885317A (en) | Path tracking control method, system and computer readable storage medium | |
CN108839026A (en) | A kind of mobile robot visual servo tracking forecast Control Algorithm | |
CN112207834B (en) | Robot joint system control method and system based on disturbance observer | |
CN105867138B (en) | A kind of stabilized platform control method and device based on PID controller | |
CN111553239A (en) | Robot joint visual servo control method, terminal device and storage medium | |
Ribeiro et al. | Nonlinear model predictive visual path following control to autonomous mobile robots | |
SE1850676A1 (en) | Method, system and computer program for controlling dynamic manipulations by a robot | |
Shi et al. | Adaptive dynamic surface asymptotic tracking control of uncertain strict‐feedback systems with guaranteed transient performance and accurate parameter estimation | |
CN109857108B (en) | Mobile robot track tracking method and system based on internal model control algorithm | |
CN114274147B (en) | Target tracking control method and device, mechanical arm control equipment and storage medium | |
CN115213897A (en) | Trajectory compensation method, apparatus and computer-readable storage medium | |
CN107398903B (en) | Track control method for industrial mechanical arm execution end | |
CN114179089A (en) | Robust region tracking control method for mechanical arm | |
CN115981159A (en) | Generalized improved active disturbance rejection control method based on model assistance and similar Smith estimation | |
CN116466589A (en) | Path tracking control method and device based on active disturbance rejection model, storage medium and electronic equipment | |
CN114355976B (en) | Method for controlling unmanned aerial vehicle to complete track tracking under wind disturbance based on learning | |
CN114114903B (en) | Cricket system integral terminal sliding mode control method based on variable exponent power approach law | |
CN112346344B (en) | Trajectory tracking control method for wheeled robot | |
Kim et al. | Asymptotic output tracking in a class of non‐minimum phase nonlinear systems via learning‐based inversion | |
CN115008450A (en) | Robot control method and device, electronic equipment and storage medium | |
CN117506936B (en) | Mechanical arm control method and master-slave control system | |
Ruiz et al. | Model learning predictive control for batch processes:: A Reactive Batch Distillation Column Case Study |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |