CN113671950B - Vehicle track tracking control method based on pose convergence algorithm - Google Patents
Vehicle track tracking control method based on pose convergence algorithm Download PDFInfo
- Publication number
- CN113671950B CN113671950B CN202110856615.4A CN202110856615A CN113671950B CN 113671950 B CN113671950 B CN 113671950B CN 202110856615 A CN202110856615 A CN 202110856615A CN 113671950 B CN113671950 B CN 113671950B
- Authority
- CN
- China
- Prior art keywords
- pose
- vehicle
- posture
- target position
- convergence
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 25
- 230000000295 complement effect Effects 0.000 claims description 3
- 230000000694 effects Effects 0.000 abstract description 6
- 238000010586 diagram Methods 0.000 description 6
- 238000009795 derivation Methods 0.000 description 3
- 206010039203 Road traffic accident Diseases 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 230000004927 fusion Effects 0.000 description 1
- 239000013598 vector Substances 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
Abstract
The invention belongs to the technical field of unmanned automobiles, and discloses a vehicle track tracking control method based on a pose convergence algorithm, which comprises the following steps: step S1, determining the current position and the posture P of the vehicle 0 Attitude P with target position t According to the current position posture P 0 And target position posture P t Deducing a corresponding equivalent rotation angle delta, namely the control rate of a pose convergence algorithm; step S2, the vehicle moves according to the control rate of the pose convergence algorithm described in step S1 until reaching the target position pose P t . The invention comprehensively considers the position and the course angle on the expected track, can couple the position and the course angle on the expected track, simultaneously track, can directly reach the target position without planning the track, has better control effect, and has particularly obvious effect in the tracking control process of the vehicle lane change and the parking working condition.
Description
Technical Field
The invention belongs to the technical field of unmanned automobiles, and particularly relates to a vehicle track tracking control method based on a pose convergence algorithm.
Background
With the continuous development of the economy and society, the vehicle conservation amount in China rises year by year. The data show that hundreds of thousands of traffic accidents occur in China every year, and about hundreds of thousands of people die, which accounts for one ten thousandth of the general population. Many factors such as weather, traffic environment, driving vehicles, and human factors affect driving safety, wherein human factors are the most important factors causing traffic accidents. To fundamentally reduce the number of road safety accidents, it is necessary to study the vehicle driver who is dominated by "non-human", and it is generally considered that an intelligent driving system using a multi-sensor fusion as a sensing system is the best choice, and a planning control technique is an important technique of the intelligent driving system, which calculates the motion of the vehicle through a control algorithm based on the sensing positioning information, and is executed by a controller.
At present, for the path tracking transverse control of a vehicle, a control algorithm based on geometric model derivation mainly comprises the following steps: pure tracking algorithm and Stanley algorithm. The former only controls the position point on the vehicle arrival path as much as possible without considering the vehicle body posture when the position point is reached, so that a large course angle error may exist when the vehicle arrives at the position point; the latter, while taking into account both the position error and the heading angle error, simply superimposes the two linearly in the control quantity, and does not take into account the inherent relationship of the two errors.
Therefore, if there is a control algorithm that can couple the position and the heading angle and track them simultaneously, it is possible to eliminate the errors generated in the path tracking process.
Disclosure of Invention
The invention aims to provide a vehicle track tracking control method based on a pose convergence algorithm, which can couple a position and a course angle and track the position and the course angle simultaneously, and simultaneously eliminates errors generated in a path tracking process of the position and the course angle.
The technical scheme adopted by the invention is as follows: a vehicle track tracking control method based on a pose convergence algorithm comprises the following steps:
step S1, determining the current position and the posture P of the vehicle 0 Attitude P with target position t According to the current position posture P 0 And target position posture P t The corresponding equivalent rotation angle delta is deduced by the geometric model of the model (C), namely the control rate of the pose convergence algorithm.
Specifically, the position posture of the vehicle includes the position coordinates (x, y) of the equivalent rear wheels of the vehicle and the current position posture P of the vehicle 0 Attitude P with target position t A difference θ in yaw angle of (2); the equivalent rotation angle δ is determined by the formula (1):
wherein L is the front-rear wheelbase and x of the automobile r Representing the position of the target relative to the current timeProjection of the front position in the direction of the target pose, y r Representing the projection of the target position relative to the current position in the direction of the target attitude, ε being the convergence coefficient (ε)>0)。
Step S2, the vehicle moves according to the control rate of the pose convergence algorithm described in step S1 until reaching the target position pose P t 。
In the step S1, how to output the control rate of the pose convergence algorithm to enable the vehicle to be in the pose P from the current position is directly analyzed 0 Reaching the target position posture P t Is more complex, so that the constraint can be temporarily reduced to enable the current position and the posture P of the vehicle 0 Without having to directly reach the target position pose P t But only needs to reach the target position posture P t Any position on the straight line corresponding to the gesture is needed.
Further, in the step S1, the current position and orientation P is exceeded 0 Make the current position posture P 0 Vertical line corresponding to the attitude direction and the current position and attitude P of the vehicle 0 Attitude P with target position t The angular bisector of the complementary angle corresponding to the difference theta between the yaw angles of (2) intersects at a point, and the point is taken as the center of a circle, and the center of the circle is taken as the current position posture P 0 Is the distance of the radius from the target position and the posture P t Corresponding to the intersection of the gesture direction at P et . If the target position and the attitude P of the vehicle t Just at P et Defining the state as a steady state of a pose convergence algorithm, and keeping the control rate of the current pose convergence algorithm for movement of the vehicle; otherwise, the method is called unsteady state, the control rate of the corrected pose convergence algorithm is calculated, and the system in the unsteady state is controlled to enter the steady state.
When the vehicle is in an unsteady state, the pose convergence algorithm needs to control a system in the unsteady state to enter the steady state, and a pose convergence error is defined as shown in a formula (2):
the pose convergence error e is derived with respect to the arc length:
as a result of:
the rate of change of the pose convergence error e with respect to arc length is:
and (3) making:
the corrected pose convergence algorithm control rate, namely the equivalent turning angle delta of the steering wheel, can be obtained by combining the formula (5) and the formula (6).
In step S1 of the present invention, a Lyapunov energy function is selected as formula (7):
the derivative can be obtained by:
substitution of formula (1) into formula (8) yields:
it is known from Lyapunov's law of stability that the vehicle will converge to the target position pose at an exponential approach rate over a finite arc length.
In the vehicle track tracking control method based on the pose convergence algorithm, in the step S1, the pose convergence algorithm can couple the position on the expected path with the course angle and track the position at the same time, and the target position and the pose can be directly reached without planning the path.
In the vehicle track tracking control method based on the pose convergence algorithm, in the step S2, when the vehicle is in an unsteady state, the vehicle is rapidly close to a steady state under the control of the control rate of the pose convergence algorithm. As to which steady state the vehicle body will reach, it is related to the current position and attitude of the vehicle and the convergence coefficient epsilon, and cannot be directly controlled by the control rate of the attitude convergence algorithm. In step S2, the larger the convergence coefficient epsilon, the larger the turning radius of the track arc at the time of the steady state where the vehicle finally arrives, and vice versa.
Therefore, compared with the prior art, the vehicle track tracking control method based on the pose convergence algorithm has at least the following beneficial effects: the method is simple, has small operand and easy realization, and compared with a pure tracking algorithm and a Stanley algorithm which are two control algorithms based on geometric model derivation, the algorithm comprehensively considers the position and the course angle on the expected track in a more scientific way, has better control effect, can couple the position and the course angle on the expected track, simultaneously tracks, can directly reach the target position posture without planning the track, and has obvious effect in the tracking control process of the vehicle lane change and parking working conditions.
Drawings
The invention will now be described in further detail with reference to the drawings and to specific examples.
FIG. 1 is a schematic diagram of a pose convergence algorithm in an embodiment of the invention;
FIG. 2 is a schematic diagram of a pose convergence error in an embodiment of the present invention;
FIG. 3 is a schematic diagram of the motion trail characteristics of a vehicle under the control of a pose convergence algorithm in an embodiment of the invention;
FIG. 4 is a graph showing the relationship between convergence factor and vehicle motion trajectory in the example of the present invention;
FIG. 5 is a graph of convergence factor versus vehicle trajectory curvature in an example of the invention.
In the above figures: p (P) 0 Representing the current position and attitude of the vehicle; p (P) t Representing a vehicle target position pose; p (P) et Representing that the vehicle is in a steady state of a pose convergence algorithm; θ represents the current position posture P of the vehicle 0 Attitude P with target position t The difference in yaw angle of (2); x is x r Representing a projection of the target position relative to the current position in the target pose direction; y is r Representing a projection of the target position relative to the current position in a target attitude direction; e represents the pose convergence error.
Detailed Description
Embodiments of the present invention will be described in detail below with reference to examples, but it will be understood by those skilled in the art that the following examples are only for illustrating the present invention and should not be construed as limiting the scope of the present invention. It should be noted that, in the embodiment of the present invention, all coordinate vectors follow the geodetic coordinate system.
The technical scheme adopted by the invention is as follows: a vehicle track tracking control method based on a pose convergence algorithm comprises the following steps:
as shown in the schematic view of the pose convergence algorithm of FIG. 1, in step S1, the pose P of the current position of the vehicle is determined 0 Attitude P with target position t According to the current position posture P 0 And target position posture P t The corresponding equivalent rotation angle delta is deduced by the geometric model of the model (C), namely the control rate of the pose convergence algorithm.
Specifically, the position posture of the vehicle includes the position coordinates (x, y) of the equivalent rear wheels of the vehicle and the current position posture P of the vehicle 0 Attitude P with target position t The difference θ between the yaw angles of (a) and the equivalent rotation angle δ is determined by the formula (1):
wherein L is the front-rear wheelbase and x of the automobile r Representing the projection of the target position relative to the current position in the target pose direction, y r Representing the target position relative to the current position in the direction of the target attitudeProjection, ε is the convergence coefficient (ε)>0)。
Step S2, the vehicle moves according to the control rate of the pose convergence algorithm described in step S1 until reaching the target position pose P t 。
In the step S1, how to output the control rate of the pose convergence algorithm to enable the vehicle to be in the pose P from the current position is directly analyzed 0 Reaching the target position posture P t Is more complex, so that the constraint can be temporarily reduced to enable the current position and the posture P of the vehicle 0 Without having to directly reach the target position pose P t But only needs to reach the target position posture P t Any position on the straight line corresponding to the gesture is needed.
Further, in the step S1, as shown in the schematic view of the pose convergence algorithm in fig. 1, the current position pose P is exceeded 0 Make the current position posture P 0 Vertical line corresponding to the attitude direction and the current position and attitude P of the vehicle 0 Attitude P with target position t The angular bisector of the complementary angle corresponding to the difference theta between the yaw angles of (2) intersects at a point, and the point is taken as the center of a circle, and the center of the circle is taken as the current position posture P 0 Is the distance of the radius from the target position and the posture P t Corresponding to the intersection of the gesture direction at P et . If the target position and the attitude P of the vehicle t Just at P et Defining the state as a steady state of a pose convergence algorithm, and keeping the control rate of the current pose convergence algorithm for movement of the vehicle; otherwise, the method is called unsteady state, the control rate of the corrected pose convergence algorithm is calculated, and the system in the unsteady state is controlled to enter the steady state.
As shown in fig. 2, when the vehicle is in an unsteady state, the pose convergence algorithm needs to control the system in the unsteady state to enter the steady state, and the pose convergence error is defined as formula (2):
the pose convergence error e is derived with respect to the arc length:
as a result of:
the rate of change of the pose convergence error e with respect to arc length is:
and (3) making:
the corrected pose convergence algorithm control rate, namely the equivalent turning angle delta of the steering wheel, can be obtained by combining the formula (5) and the formula (6).
In step S1 of the present invention, a Lyapunov energy function is selected as formula (7):
the derivative can be obtained by:
substitution of formula (1) into formula (8) yields:
it is known from Lyapunov's law of stability that the vehicle will converge to the target position pose at an exponential approach rate over a finite arc length.
In the vehicle track tracking control method based on the pose convergence algorithm, in the step S1, the pose convergence algorithm can couple the position on the expected path with the course angle and track the position at the same time, and the target position and the pose can be directly reached without planning the path.
In the vehicle track tracking control method based on the pose convergence algorithm, in the step S2, when the vehicle is in an unsteady state, the vehicle is rapidly close to a steady state under the control of the control rate of the pose convergence algorithm; as to which steady state the vehicle body will reach, the vehicle body is related to the current position and posture of the vehicle and the convergence coefficient epsilon, and cannot be directly controlled by the control rate of the posture convergence algorithm, such as the schematic diagram of the motion trail feature of the vehicle under the control of the posture convergence algorithm of fig. 3, and the relationship diagram of the convergence coefficient and the motion trail of the vehicle of fig. 4. In step S2, the larger the convergence coefficient epsilon, the larger the turning radius of the track arc is when the vehicle finally reaches a steady state, and conversely, the relationship diagram of the convergence coefficient and the vehicle motion track curvature is shown in fig. 5.
Therefore, compared with the prior art, the vehicle track tracking control method based on the pose convergence algorithm has at least the following beneficial effects: the method is simple, has small operand and easy realization, and compared with a pure tracking algorithm and a Stanley algorithm which are two control algorithms based on geometric model derivation, the algorithm comprehensively considers the position and the course angle on the expected track in a more scientific way, has better control effect, can couple the position and the course angle on the expected track, simultaneously tracks, can directly reach the target position posture without planning the track, and has obvious effect in the tracking control process of the vehicle lane change and parking working conditions.
While the invention has been described in detail in this specification with reference to the general description and the specific embodiments thereof, it will be apparent to one skilled in the art that modifications and improvements can be made thereto. Accordingly, such modifications or improvements may be made without departing from the spirit of the invention and are intended to be within the scope of the invention as claimed.
Claims (1)
1. The vehicle track tracking control method based on the pose convergence algorithm is characterized by comprising the following steps of:
step S1, determining the current position and the posture P of the vehicle 0 Attitude P with target position t According to the current position posture P 0 And target position posture P t Deducing a corresponding equivalent rotation angle delta, namely the control rate of a pose convergence algorithm;
the position and posture of the vehicle include the position coordinates (x, y) of the equivalent rear wheel of the vehicle and the current position and posture P of the vehicle 0 Attitude P with target position t A difference θ in yaw angle of (2); the equivalent rotation angle δ is determined by the formula (1):
wherein L is the front-rear wheelbase and x of the automobile r Representing the projection of the target position relative to the current position in the target pose direction, y r Representing the projection of the target position relative to the current position in the direction of the target attitude, epsilon being the convergence coefficient>0；
Past current position attitude P 0 Make the current position posture P 0 Vertical line corresponding to the attitude direction and the current position and attitude P of the vehicle 0 Attitude P with target position t The angular bisector of the complementary angle corresponding to the difference theta between the yaw angles of (2) intersects at a point, and the point is taken as the center of a circle, and the center of the circle is taken as the current position posture P 0 Is the distance of the radius from the target position and the posture P t Corresponding to the intersection of the gesture direction at P et ；
If the vehicle target position is in the target position posture P t Just at P et Defining the state as a steady state of a pose convergence algorithm, and keeping the control rate of the current pose convergence algorithm for movement of the vehicle; otherwise, the vehicle is called as unsteady state, the control rate of the pose convergence algorithm to be corrected is calculated, and the vehicle in the unsteady state is controlled to enter the steady state;
when the vehicle is in an unsteady state, the pose convergence algorithm needs to control a system in the unsteady state to enter the steady state, and a pose convergence error is defined as shown in a formula (2):
the pose convergence error e is derived with respect to the arc length:
as a result of:
the rate of change of the pose convergence error e with respect to arc length is:
and (3) making:
the corrected pose convergence algorithm control rate, namely the equivalent turning angle delta of the steering wheel, can be obtained by combining the formula (5) and the formula (6);
step S2, the vehicle moves according to the control rate of the pose convergence algorithm described in step S1 until reaching the target position pose P t 。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110856615.4A CN113671950B (en) | 2021-07-28 | 2021-07-28 | Vehicle track tracking control method based on pose convergence algorithm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110856615.4A CN113671950B (en) | 2021-07-28 | 2021-07-28 | Vehicle track tracking control method based on pose convergence algorithm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113671950A CN113671950A (en) | 2021-11-19 |
| CN113671950B true CN113671950B (en) | 2024-02-02 |
Family
ID=78540396
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110856615.4A Active CN113671950B (en) | 2021-07-28 | 2021-07-28 | Vehicle track tracking control method based on pose convergence algorithm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113671950B (en) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2010155546A (en) * | 2008-12-26 | 2010-07-15 | Toyota Motor Corp | Device and method for controlling vehicle |
| CN107298103A (en) * | 2017-07-03 | 2017-10-27 | 厦门大学 | A kind of automatic lane-change hierarchy system of intelligent electric automobile and method |
| CN108319138A (en) * | 2018-01-29 | 2018-07-24 | 哈尔滨工程大学 | A kind of sliding formwork of drive lacking unmanned boat-contragradience double loop Trajectory Tracking Control method |
| CN109703553A (en) * | 2019-01-30 | 2019-05-03 | 长安大学 | A kind of automatic parking method based on towing point tracking |
| CN109857100A (en) * | 2019-01-09 | 2019-06-07 | 合肥工业大学 | A kind of complex track tracking control algorithm based on the method for inversion and fast terminal sliding formwork |
| CN111547066A (en) * | 2020-04-27 | 2020-08-18 | 中汽研(天津)汽车信息咨询有限公司 | Vehicle trajectory tracking method, device, equipment and storage medium |
| CN111610780A (en) * | 2019-02-25 | 2020-09-01 | 广州汽车集团股份有限公司 | Automatic driving vehicle path tracking control method and device |
| CN111890951A (en) * | 2020-08-07 | 2020-11-06 | 吉林大学 | Intelligent electric automobile trajectory tracking and motion control method |
-
2021
- 2021-07-28 CN CN202110856615.4A patent/CN113671950B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2010155546A (en) * | 2008-12-26 | 2010-07-15 | Toyota Motor Corp | Device and method for controlling vehicle |
| CN107298103A (en) * | 2017-07-03 | 2017-10-27 | 厦门大学 | A kind of automatic lane-change hierarchy system of intelligent electric automobile and method |
| CN108319138A (en) * | 2018-01-29 | 2018-07-24 | 哈尔滨工程大学 | A kind of sliding formwork of drive lacking unmanned boat-contragradience double loop Trajectory Tracking Control method |
| CN109857100A (en) * | 2019-01-09 | 2019-06-07 | 合肥工业大学 | A kind of complex track tracking control algorithm based on the method for inversion and fast terminal sliding formwork |
| CN109703553A (en) * | 2019-01-30 | 2019-05-03 | 长安大学 | A kind of automatic parking method based on towing point tracking |
| CN111610780A (en) * | 2019-02-25 | 2020-09-01 | 广州汽车集团股份有限公司 | Automatic driving vehicle path tracking control method and device |
| CN111547066A (en) * | 2020-04-27 | 2020-08-18 | 中汽研(天津)汽车信息咨询有限公司 | Vehicle trajectory tracking method, device, equipment and storage medium |
| CN111890951A (en) * | 2020-08-07 | 2020-11-06 | 吉林大学 | Intelligent electric automobile trajectory tracking and motion control method |
Non-Patent Citations (1)
| Title |
|---|
| 基于模拟退火算法的无人驾驶车辆轨迹跟踪控制;王浩;《重庆理工大学学报》;第29卷(第11期);第106-113页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113671950A (en) | 2021-11-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110001637B (en) | Unmanned vehicle path tracking control device and control method based on multipoint tracking | |
| WO2020187257A1 (en) | Vehicle abnormal lane change control method, device and system | |
| CN111610780B (en) | Automatic driving vehicle path tracking control method and device | |
| CN112733270B (en) | System and method for predicting vehicle running track and evaluating risk degree of track deviation | |
| WO2020143288A1 (en) | Autonomous vehicle decision-making system under complex operating conditions, and trajectory planning method therefor | |
| CN110568758B (en) | Parameter self-adaptive transverse motion LQR control method for automatically driving automobile | |
| CN110703754B (en) | Path and speed highly-coupled trajectory planning method for automatic driving vehicle | |
| CN108732921B (en) | Transverse extension preview switching control method for automatic driving automobile | |
| CN111409641A (en) | Trajectory tracking control method and system based on longitudinal and transverse coordination | |
| WO2009155228A1 (en) | Path generation algorithm for automated lane centering and lane changing control system | |
| CN113104037B (en) | Method and system for determining steering angle of vehicle steering wheel | |
| CN112462760B (en) | Double-steering-wheel AGV path tracking method | |
| CN109017793A (en) | The navigation of autonomous trick vehicle and control method based on antero posterior axis fusion reference | |
| CN110851916B (en) | Vehicle kinematics man-vehicle-road closed loop system suitable for road with any curvature | |
| CN110162046A (en) | Unmanned vehicle path following method based on event trigger type model predictive control | |
| CN115551756A (en) | Path control module, associated path control device and associated method | |
| CN112660124A (en) | Collaborative adaptive cruise control method for lane change scene | |
| CN113619574A (en) | Vehicle avoidance method and device, computer equipment and storage medium | |
| CN109292018A (en) | Four-wheel steering Trajectory Tracking Control method based on coaxial-type wheel leg structure | |
| CN114852085A (en) | Automatic vehicle driving track planning method based on road right invasion degree | |
| CN113671950B (en) | Vehicle track tracking control method based on pose convergence algorithm | |
| CN212828326U (en) | Trajectory tracking control system based on longitudinal and transverse coordination | |
| CN114291117B (en) | Four-wheel steering unmanned vehicle track tracking control method | |
| Qian et al. | Research on automatic parking system based on linear quadratic regulator | |
| CN211107381U (en) | Intelligent automobile path tracking control system based on wolf algorithm |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |