CN113721454B - Articulated vehicle path tracking control method - Google Patents
Articulated vehicle path tracking control method Download PDFInfo
- Publication number
- CN113721454B CN113721454B CN202111035734.XA CN202111035734A CN113721454B CN 113721454 B CN113721454 B CN 113721454B CN 202111035734 A CN202111035734 A CN 202111035734A CN 113721454 B CN113721454 B CN 113721454B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- path
- error
- front axle
- axle
- 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 23
- 238000004364 calculation method Methods 0.000 claims description 6
- 230000008859 change Effects 0.000 claims description 5
- 238000004422 calculation algorithm Methods 0.000 claims description 4
- 230000007246 mechanism Effects 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 3
- 238000007619 statistical method Methods 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008602 contraction Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000036541 health Effects 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000010355 oscillation Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
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
- G05B11/00—Automatic controllers
- G05B11/01—Automatic controllers electric
- G05B11/36—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential
- G05B11/42—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential for obtaining a characteristic which is both proportional and time-dependent, e.g. P. I., P. I. D.
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Steering Control In Accordance With Driving Conditions (AREA)
Abstract
The invention discloses an articulated vehicle path tracking control method, which comprises the following steps: step S1, calculating a rear vehicle body path reference point according to a path reference point of a front vehicle body and an articulated vehicle model; s2, obtaining the current position of the vehicle; s3, finding out the nearest path points to the front axle and the rear axle; s4, searching N points forward according to the nearest path point to serve as pre-aiming points of the front axle and the rear axle; s5, calculating a front axle pre-aiming position error, a course error, a rear axle pre-aiming position error and a course error; s6, inputting all errors into a path tracking controller; and S7, calculating the control quantity according to the PID controller to carry out steering control on the vehicle. The invention discloses a method for planning paths of front and rear axles of an articulated vehicle, calculating the pre-aiming position deviation and course deviation of the front and rear axles, calculating the control quantity through a controller to carry out steering control on the articulated vehicle, and realizing path tracking control of the articulated vehicle.
Description
Technical Field
The invention relates to the technical field of vehicle running path control, in particular to an articulated vehicle path tracking control method.
Background
The articulated vehicle consists of a front part vehicle body and a rear part vehicle body, the middle part of the articulated vehicle is connected by an articulated mechanism, and the structure can reduce the turning radius, improve the operation stability of the vehicle and better adapt to the non-structural environment. However, due to the harsh working environment and frequent acceleration and steering operations, the health and the working efficiency of the driver are affected. Unmanned driving of an articulated vehicle is a way to improve the situation, and a path tracking control method is one of core technologies for realizing automatic driving. Therefore, a path tracking control method suitable for an articulated vehicle having good tracking accuracy and tracking stability is very important for unmanned driving of the articulated vehicle.
The conventional path control method only considers the error of a front axle or the error of a rear axle, and then performs tracking control on the vehicle through a control algorithm. The hinge mechanism between the front and rear bodies of the articulated vehicle is controlled to rotate with a degree of freedom, so that the front and rear bodies can deflect to a certain degree, namely, the hinge angle, and the phenomena of ' snaking ', tail sway ' and the like are easily generated because the vehicle has poor stable running capability due to the hinge angle. The common path control method is generally applied to rigid vehicle bodies and structured roads, is not suitable for articulated vehicles, makes the accurate tracking of the path more difficult due to the articulated structure, and cannot ensure the safe and stable running of the articulated vehicle if the accurate path tracking cannot be realized, so that the unmanned driving of the articulated vehicle cannot be realized.
Disclosure of Invention
The invention aims to provide an articulated vehicle path tracking control method to solve the problem that two vehicle bodies cannot track the same path well.
In order to achieve the purpose, the invention provides the following technical scheme:
an articulated vehicle path following control method comprising the steps of: step S1, calculating a path reference point of a rear vehicle body according to an existing reference path point as a reference point of a front vehicle body; s2, obtaining the current position of the vehicle; s3, finding a path point closest to the front axle on the front axle reference path, and finding a path point closest to the rear axle on the rear axle reference path; s4, searching N points in front of the forward driving by taking the point closest to the front axle as a starting point to serve as a pre-aiming point, and calculating the pre-aiming point of the rear axle in the same way; s5, calculating a pre-aiming position error and a course error of a front axle of the vehicle, and calculating a pre-aiming position error and a course error of a rear axle of the vehicle; s6, inputting the calculated pre-aiming error and course error of the front axle and the pre-aiming position error and course error of the rear axle into a path tracking controller; s7, the path tracking controller calculates the control quantity of the articulated vehicle to carry out steering control on the vehicle; and S8, when the vehicle is controlled to reach the next sampling time, repeating the steps S2-S7.
On the basis of the technical scheme, the invention also provides the following optional technical scheme:
in one alternative: the path tracking controller comprises a fuzzy reasoning module and a PID controller, wherein the fuzzy reasoning module is used for roughly reasoning out parameters of the PID controller, and the PID controller calculates through a PID algorithm to obtain a controlled quantity.
In one alternative: the path tracking controller also comprises a weight distribution module, and the weight distribution module is used for distributing the weight of the control quantity calculated by the front and rear bridge PID controllers in the total control quantity.
Compared with the prior art, the invention has the following beneficial effects:
(1) The control method can realize unmanned driving of the articulated vehicle, does not need operation of a driver, improves the working efficiency of the articulated vehicle, and reduces the incidence rate of driving accidents.
(2) Due to the special articulated structure, the driving routes of the front axle body and the rear axle body are not on the same route, the route planning is carried out on the front axle and the rear axle body of the articulated vehicle according to the steering model of the articulated vehicle, and compared with the traditional route planning only on the front axle or only on the rear axle, the method adopts the front axle and rear axle double route planning, the tracking error of the front axle and the rear axle can be more accurately solved, and the route tracking of the articulated vehicle can be better realized.
(3) The pre-aiming control and the PID control are combined, so that the tracking error in front of the vehicle can be pre-judged, and the vehicle can be controlled in advance.
(4) In the aspect of PID control, the error of the front axle and the error of the rear axle are calculated, and compared with the traditional control method which only considers the error of the front axle, the method can enable the front and rear axles to track the upper reference path, and improves the accuracy and stability of path tracking.
The invention discloses a method for planning paths of front and rear axles of an articulated vehicle, then calculating the pre-aiming position deviation and course deviation of the front and rear axles, and calculating control quantity through a fuzzy PID controller to perform path tracking control on the articulated vehicle. Therefore, the tracking precision of the articulated vehicle can be improved, and the stable running capability of the vehicle can also be improved.
Drawings
Fig. 1 is a schematic view of the overall structure of a path tracking control flowchart in the present invention.
Fig. 2 is a schematic structural diagram of a path tracking controller according to the present invention.
Fig. 3 is a schematic structural view of an articulated vehicle steering model in the invention.
Fig. 4 is a schematic diagram of a midpoint reference path structure of a front axle in the present invention.
Fig. 5 is a schematic diagram of a rear axle midpoint reference path structure in the present invention.
Fig. 6 is a schematic view of a deviated structure of the front axle of the articulated vehicle according to the present invention.
Fig. 7 is a schematic view of a deviated structure of the rear axle of the articulated vehicle according to the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is further described in detail below with reference to the accompanying drawings and embodiments. The examples are given solely for the purpose of illustration and are not intended to limit the scope of the invention. Any obvious modifications or variations can be made without departing from the spirit or scope of the present invention.
An articulated vehicle path tracking control method comprising the steps of:
step S1, calculating a path reference point of a rear vehicle body according to an existing reference path point as a reference point of a front vehicle body;
s2, obtaining the current position of the vehicle;
s3, finding a path point closest to the front axle on the front axle reference path, and finding a path point closest to the rear axle on the rear axle reference path;
s4, searching N points in front of the forward driving by taking the point closest to the front axle as a starting point to serve as a pre-aiming point, and calculating the pre-aiming point of the rear axle in the same way;
s5, calculating a pre-aiming position error and a course error of a front axle of the vehicle, and calculating a pre-aiming position error and a course error of a rear axle of the vehicle;
s6, inputting the calculated pre-aiming position error and course error of the front axle and the pre-aiming position error and course error of the rear axle into a path tracking controller;
s7, calculating the control quantity of the articulated vehicle by the path tracking controller to carry out steering control on the vehicle;
s8, when the vehicle is controlled to reach the next sampling moment, repeating the steps S2-S7;
the path tracking controller comprises a fuzzy reasoning module and a PID controller, wherein the fuzzy reasoning module is used for roughly reasoning out PID parameters, and the PID controller calculates through a PID algorithm to obtain a control quantity; the path tracking controller also comprises a weight distribution module, and the weight distribution module is used for distributing the weight of the control quantity calculated by the front bridge PID controller and the rear bridge PID controller in the total control quantity;
because the articulated vehicle is composed of two vehicle bodies, the front and rear vehicle bodies are usually not on the same path, and path planning needs to be performed on the front and rear vehicle bodies. The reference path of the rear vehicle body can be calculated according to the reference path of the front vehicle body.
The turning radius can be calculated from three adjacent reference points of the driving path, and the reference point of the path is set as P fi (X fi ，Y fi ) Two adjacent reference points thereof are P fi-1 (X fi-1 ，Y fi-1 )，P fi+1 (X fi+1 ，Y fi+1 )
As can be seen from FIG. 3, the steering radius R of the front axle of the loader f The relationship to the articulation angle γ is:
R f ＝AO
AD＝l f cosγ+l r
R＝R f
when the vehicle runs forwards, the path of the rear axle of the vehicle is obtained as follows:
X ri ＝X fi -l f cosθ 1 -l r cos(γ-θ 1 )
Y ri ＝Y fi -l f sinθ 1 -l r sin(γ-θ 1 )
the current position of the articulated vehicle is the abscissa x of the center point of the front axle f Longitudinal coordinate y of the center point of the front axle f Heading angle theta of front axle f And the articulation angle gamma can be obtained from a navigation sensor and a rotation angle sensor, the abscissa x of the center point of the rear axle r Longitudinal coordinate y of middle point of rear axle r Course angle theta of rear axle r Can be calculated by the following formula:
x r ＝x f -l f cosθ f -l r cosθ r
y r ＝y r -l f sinθ f -l r sinθ r
θ r ＝θ f -γ
the current vehicle front axle midpoint D at the current moment t can be calculated according to the following formula f (x f (t)，y f (t)) and the front axle reference path f Find D f Minimum path point, the point P fi (X fi ，Y fi ) As the closest path reference point to the front axle center:
according to the calculation of the nearest road to the center of the front axleThe path reference point P closest to the center of the rear axle can be calculated ri (X ri ，Y ri )。
With the found path reference point P closest to the center of the front axle fi (X fi ，Y fi ) As a starting point, according to the running speed v (t) of the articulated vehicle, kappa is a preview constant, kappa v (t) is a preview distance, k points are searched forward along a reference path as a preview point P fi+k (X fi+k ，Y fi+k ) As shown in fig. 4;
according to the method for calculating the preview point of the center of the front axle, the preview point P of the center of the rear axle can be calculated ri+k (X ri+k ，Y ri+k ) As shown in fig. 5;
the method comprises the steps of simplifying a front axle of a vehicle into a point, establishing a vehicle coordinate system by taking the driving direction of the front axle of the vehicle as an x axis and the left side of a cab as a y axis, converting the coordinate of a pre-aiming point into the vehicle coordinate system as shown in figure 6, solving the following formula, and then calculating the position error and the course error of the front axle.
y ri+k ＝-(X fi+k -x f (t))sinθ f +(Y fi+k +y f (t))cosθ f
Position deviation d of front axle f Comprises the following steps:
d f ＝y ri+k
namely:
d f ＝-(X fi+k -x f (t))sinθ f +(Y fi+k +y f (t))cosθ f
heading deviation of front axle
Comprises the following steps:
from the calculation of the deviation of the front axle, the deviation of the rear axle can likewise be calculated, the deviation of the rear axle of the articulated vehicle being shown in fig. 7, the positional deviation d r Comprises the following steps:
d r ＝-(X ri+k -x r (t))sinθ r +(Y ri+k +y r (t))cosθ r
course deviation of rear axle
Comprises the following steps:
the calculation formula of the PID controller is as follows:
the fuzzy controller writes corresponding fuzzy rules according to corresponding control requirements, and the control idea of the articulated vehicle is as follows:
(1) When the position deviation and the course deviation are large, K is used for avoiding large overshoot p Should be kept as small as possible, the deviation begins to decrease as the articulated vehicle is driven, in which case K is increased p . When the position deviation is small and the course deviation is large, K p Should continue to increase, the articulated vehicle can be made to realign ahead of time, reducing the overshoot of the vehicle.
(2)K i The effect of (1) is to eliminate the stability error of the system and improve the control precision of the system, but also influence the vibration amplitude of the system. When the position deviation and the course deviation are relatively large, K i A smaller value is selected to prevent the system from generating large oscillation; when the deviation is smaller, K is increased i The tracking accuracy can be improved.
(3)K d The overshoot of the system can be suppressed. When the position deviation and the course deviation are large, K d The overshoot of the system can be reduced by taking a larger value; when the deviation is small, K d Should take a smaller value, when K i The function can be played more, and the articulated vehicle can better track the reference path.
The calculated position deviation d of the front axle f Course deviation from front axle
Is input into a fuzzy PID controller and then is input into a fuzzy PID controller, the output being the change delta of the articulation angle 1 。
Similarly, the calculated positional deviation d of the rear axle r Course deviation from rear axle
Is input into a fuzzy PID controller and then is input into a fuzzy PID controller, the output being the variation delta of the articulation angle 2 。
The amount of change delta of the articulation angle to be determined 1 And delta 2 The weighted sum is performed, and the change amount Δ δ of the articulation angle is obtained according to the following calculation formula.
The steering of the articulated vehicle can be realized by controlling the contraction of the hydraulic cylinder, so that the vehicle accurately and smoothly tracks the upper reference path.
The above description is only for the specific embodiments of the present disclosure, but the scope of the present disclosure is not limited thereto, and any person skilled in the art can easily think of the changes or substitutions within the technical scope of the present disclosure, and shall cover the scope of the present disclosure. Therefore, the protection scope of the present disclosure shall be subject to the protection scope of the claims.
Claims (4)
1. An articulated vehicle path following control method, characterized by comprising the steps of:
step S1, calculating a path reference point of a rear vehicle body according to an existing reference path point serving as a reference point of a front vehicle body;
s2, obtaining the current position of the vehicle;
s3, finding a path point closest to the front axle on the front axle reference path, and finding a path point closest to the rear axle on the rear axle reference path;
s4, searching N points in front of the forward driving by taking a point closest to the front axle as a starting point to serve as a pre-aiming point, and calculating the pre-aiming point of the rear axle in the same way;
s5, calculating a pre-aiming position error and a course error of a front axle of the vehicle, and calculating a position error and a course error of a rear axle of the vehicle;
s6, inputting the calculated pre-aiming error and course error of the front axle and the calculated position error and course error of the rear axle into a path tracking controller;
simplifying a front axle of a vehicle into a point, establishing a vehicle coordinate system by taking the running direction of the front axle of the vehicle as an x axis and the left side of a cab as a y axis, converting the coordinate of a pre-aiming point into the vehicle coordinate system, solving the following formula, and then calculating the position error and the course error of the front axle;
y ri+k ＝-(X fi+k -x f (t))sinθ f +(Y fi+k +y f (t))cosθ f
position deviation d of front axle f Comprises the following steps:
d f ＝y ri+k
namely:
d f ＝-(X fi+k -x f (t))sinθ f +(Y fi+k +y f (t))cosθ f
course deviation of front axle
Comprises the following steps:
from the calculation of the deviation of the front axle, the deviation, position deviation d of the rear axle can likewise be calculated r Comprises the following steps:
d r ＝-(X ri+k -x r (t))sinθ r +(Y ri+k +y r (t))cosθ r
course deviation of rear axle
Comprises the following steps:
s7, the path tracking controller calculates the articulated angular velocity of the articulated vehicle to control the steering of the vehicle; wherein
The calculated position deviation d of the front axle f Course deviation from front axle
Is input into a fuzzy PID controller and then is input into a fuzzy PID controller, the output being the variation delta of the articulation angle 1 ；
Similarly, the calculated position deviation d of the rear axle r Course deviation from rear axle
Is input into a fuzzy PID controller and then is input into a fuzzy PID controller, the output being the variation delta of the articulation angle 2 ；
The amount of change delta of the articulation angle to be determined 1 And delta 2 And carrying out weighted summation to obtain the change quantity delta of the articulation angle according to the following calculation formula:
and S8, steering control is carried out on the vehicle, and when the vehicle is controlled to reach the next sampling time, the steps S2-S7 are repeated.
2. The articulated vehicle path tracking control method according to claim 1, wherein the points searched in step S4 are at least six points.
3. The articulated vehicle path tracking control method according to claim 1, wherein the path tracking controller comprises a fuzzy inference module, a PID controller and an execution mechanism, the fuzzy inference module is used for roughly inferring the calculated error, and the PID controller performs statistical analysis through an algorithm and obtains an accurate reference path; and the executing mechanism is used for obtaining a reference path according to the PID controller to control the vehicle to run.
4. The articulated vehicle path tracking control method according to claim 3, wherein the path tracking controller further comprises a weight distribution module for distributing the weight of the pre-aiming error, the heading error of the front axle and the position error and the heading error of the rear axle in the total error.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111035734.XA CN113721454B (en) | 2021-09-03 | 2021-09-03 | Articulated vehicle path tracking control method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111035734.XA CN113721454B (en) | 2021-09-03 | 2021-09-03 | Articulated vehicle path tracking control method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113721454A CN113721454A (en) | 2021-11-30 |
| CN113721454B true CN113721454B (en) | 2023-02-24 |
Family
ID=78681668
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111035734.XA Active CN113721454B (en) | 2021-09-03 | 2021-09-03 | Articulated vehicle path tracking control method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113721454B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117284277B (en) * | 2023-11-27 | 2024-02-13 | 理工雷科智途(北京)科技有限公司 | Underground articulated unmanned vehicle path tracking method based on hierarchical steering |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10239555B2 (en) * | 2015-11-19 | 2019-03-26 | Agjunction Llc | Single-mode implement steering |
| WO2021135902A1 (en) * | 2019-12-30 | 2021-07-08 | 山东海洋能源有限公司 | Lifting system with three-way motion compensation function |
| CN111857157A (en) * | 2020-08-03 | 2020-10-30 | 北京享云智汇科技有限公司 | Method for adjusting motion path of vibratory roller and control system |
-
2021
- 2021-09-03 CN CN202111035734.XA patent/CN113721454B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN113721454A (en) | 2021-11-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110001637B (en) | Unmanned vehicle path tracking control device and control method based on multipoint tracking | |
| CN111522337B (en) | Multi-drive-wheel AGV navigation method based on fuzzy control | |
| Noguchi et al. | Development of a master–slave robot system for farm operations | |
| CN108732921B (en) | Transverse extension preview switching control method for automatic driving automobile | |
| CN111006667B (en) | Automatic driving track generation system under high-speed scene | |
| CN111596671A (en) | Unmanned intelligent sweeper track tracking control method and system | |
| CN110006419A (en) | Vehicle track tracking point determination method based on preview | |
| CN113104037B (en) | Method and system for determining steering angle of vehicle steering wheel | |
| CN111487976B (en) | Backing track tracking method | |
| CN112109732A (en) | Intelligent driving self-adaptive curve pre-aiming method | |
| CN110244718B (en) | Intelligent patrol trolley capable of automatically avoiding obstacles | |
| CN112462760A (en) | Double-steering-wheel AGV path tracking method | |
| CN113721454B (en) | Articulated vehicle path tracking control method | |
| CN109292018A (en) | Four-wheel steering Trajectory Tracking Control method based on coaxial-type wheel leg structure | |
| CN111522237A (en) | Obstacle avoidance control method for semitrailer | |
| CN112731932B (en) | Path tracking method of mobile robot | |
| JP3267323B2 (en) | Unmanned vehicle control device | |
| CN114879698B (en) | Robot driving vehicle obstacle avoidance method based on improved artificial potential field and MPC | |
| CN113914406B (en) | Electric loader path tracking system and control method | |
| Miao et al. | Intelligent vehicle obstacle avoidance path-tracking control based on adaptive model predictive control | |
| CN211107381U (en) | Intelligent automobile path tracking control system based on wolf algorithm | |
| CN209833810U (en) | Six-crawler-vehicle self-adaptive traveling system based on RTK | |
| CN117519190B (en) | Novel articulated vehicle control method | |
| CN2700183Y (en) | Mechanical guiding type automatic guided vehicle guiding device | |
| CN112947442B (en) | Finite time convergence vehicle formation controller and design method |
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 |