CN113721454B - A Path Tracking Control Method for an Articulated Vehicle - Google Patents

Abstract

The invention discloses a path tracking control method for an articulated vehicle. The method includes the following steps: step S1, calculating the path reference point of the rear vehicle body according to the path reference point of the front vehicle body and the articulated vehicle model; step S2, obtaining the path reference point of the vehicle body Current position; step S3, find the path point closest to the front and rear axles; step S4, search for N points forward according to the nearest path point as the preview point of the front and rear axles; step S5, calculate the preview of the front axle Position error, heading error, rear axle preview position error and heading error; step S6, input all errors into the path tracking controller; step S7, control the steering of the vehicle according to the control amount calculated by the PID controller. The invention patent plans the path of the front and rear axles of the articulated vehicle, calculates the preview position deviation and heading deviation of the front and rear axles, and calculates the control amount through the controller to control the steering of the articulated vehicle to realize the path tracking control of the articulated vehicle.

Description

A Path Tracking Control Method for an Articulated Vehicle

technical field

The invention relates to the technical field of vehicle driving path control, in particular to an articulated vehicle path tracking control method.

Background technique

The articulated vehicle is composed of two parts, the front and the rear, and the middle is connected by a hinge mechanism. This structure can reduce the turning radius, improve the handling stability of the vehicle, and better adapt to the non-structural environment. However, due to its harsh working environment and frequent manipulations such as acceleration and steering, it will affect the driver's health and work efficiency. The unmanned driving of articulated vehicles is the way to improve this situation, and the path following control method is one of the core technologies to realize automatic driving. Therefore, a path-following control method suitable for articulated vehicles with better tracking accuracy and tracking stability is very important for the unmanned driving of articulated vehicles.

The commonly used path control method only considers the error of the front axle or the error of the rear axle, and then performs tracking control on the vehicle through the control algorithm. The hinge mechanism between the front and rear bodies of an articulated vehicle is a degree of freedom of controlled rotation, which can cause a certain deflection of the front and rear bodies, that is, the articulation angle. Due to the existence of the articulation angle, the ability of the vehicle to drive stably is poor, and it is easy to produce " Snaking", "tail wagging" and other phenomena. Commonly used path control methods are generally applied to rigid car bodies and structured roads, and are not suitable for articulated vehicles. The articulated structure makes it difficult to accurately track the path. If accurate path tracking cannot be achieved, then Just can't guarantee safe, stable travel of articulated vehicle, then the unmanned driving of articulated vehicle just can't realize.

Contents of the invention

The purpose of the present invention is to provide a path tracking control method for an articulated vehicle to solve the problem that two parts of the vehicle body cannot track the same path well.

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

A path tracking control method for an articulated vehicle, comprising the following steps: step S1, calculating the path reference point of the rear body according to the existing reference path point as the reference point of the front body; step S2, obtaining the current position of the vehicle ;Step S3, find the path point closest to the front axle on the reference path of the front axle, find the path point closest to the rear axle on the reference path of the rear axle; step S4, take the point closest to the front axle as the starting point, and drive forward Search N points in front of the front of the vehicle as preview points, and similarly, the preview point of the rear axle can be calculated; Step S5, calculate the preview position error and heading error of the vehicle front axle, and calculate the preview position error and Heading error; step S6, input the calculated preview error and heading error of the front axle and the preview position error and heading error of the rear axle into the path tracking controller; step S7, the path tracking controller calculates the articulated vehicle The control amount is used to control the vehicle steering; step S8, when the control vehicle reaches the next sampling moment, repeat steps S2-S7.

On the basis of the above technical solutions, the present invention also provides the following optional technical solutions:

In an optional solution: the path tracking controller includes a fuzzy inference module and a PID controller, the fuzzy inference module is used to roughly infer the parameters of the PID controller, and the PID controller is calculated by the PID algorithm to obtain Control amount.

In an optional solution: the path tracking controller further includes a weight allocation module, the weight allocation module is configured to allocate the weight of the control amount calculated by the front and rear axle PID controllers in the total control amount.

Compared with the prior art, the beneficial effects of the present invention are as follows:

(1) The control method can realize unmanned driving of articulated vehicles without driver operation, improve the working efficiency of articulated vehicles, and reduce the incidence of driving accidents.

(2) Because of the special articulated structure, the driving routes of the front and rear axles are not on the same path. According to the steering model of the articulated vehicle, the path planning of the front and rear axles of the articulated vehicle is carried out. Or only carry out path planning for the rear axle, this method adopts the dual path planning of the front and rear axles, can more accurately calculate the tracking error of the front and rear axles, and better realize the path tracking of the articulated vehicle.

(3) The combination of preview control and PID control can predict the tracking error ahead of the vehicle and control the vehicle in advance.

(4) In terms of PID control, not only the error of the front axle is calculated, but also the error of the rear axle is calculated. Compared with the traditional control method, which only considers the error of the front axle, this method can make both the front and rear axles track the upper reference path. Improves the accuracy and stability of path tracing.

The patent of the present invention plans the path of the front and rear axles of the articulated vehicle, and then calculates the preview position deviation and heading deviation of the front and rear axles, and calculates the control amount through the fuzzy PID controller to perform path tracking control on the articulated vehicle. This improves the tracking accuracy of articulated vehicles and also improves the vehicle's ability to drive stably.

Description of drawings

FIG. 1 is a schematic diagram of the overall structure of the path tracking control flow chart in the present invention.

Fig. 2 is a structural schematic diagram of the path tracking controller in the present invention.

Fig. 3 is a structural schematic diagram of the articulated vehicle steering model in the present invention.

Fig. 4 is a schematic structural diagram of the midpoint reference path of the front axle in the present invention.

Fig. 5 is a schematic structural diagram of the midpoint reference path of the rear axle in the present invention.

Fig. 6 is a schematic diagram of the deviation structure of the front axle of the articulated vehicle in the present invention.

Fig. 7 is a schematic diagram of the deviation structure of the articulated vehicle rear axle in the present invention.

Detailed ways

In order to make the object, technical solution and advantages of the present invention clearer, the present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. The various embodiments listed in the present invention are only used to illustrate the present invention, and are not intended to limit the scope of the present invention. Any obvious modifications or changes made to the present invention do not depart from the spirit and scope of the present invention.

A path tracking control method for an articulated vehicle, comprising the following steps:

Step S1, according to the existing reference path point as the reference point of the front body, calculate the path reference point of the rear body;

Step S2, obtaining the current position of the vehicle;

Step S3, find the path point closest to the front axle on the reference path of the front axle, and find the path point closest to the rear axle on the reference path of the rear axle;

Step S4, with the point closest to the front axle as the starting point, search for N points in front of the forward driving as preview points, similarly, the preview point of the rear axle can be calculated;

Step S5, calculating the preview position error and heading error of the front axle of the vehicle, and calculating the preview position error and heading error of the rear axle of the vehicle;

Step S6, input the calculated preview position error and heading error of the front axle and the preview position error and heading error of the rear axle into the path tracking controller;

Step S7, the path tracking controller calculates the control amount of the articulated vehicle to control the steering of the vehicle;

Step S8, when the control vehicle reaches the next sampling moment, repeat steps S2-S7;

The path tracking controller includes a fuzzy inference module and a PID controller, the fuzzy inference module is used to roughly infer PID parameters, and the PID controller calculates through a PID algorithm to obtain a control quantity; the path tracking controller also Including a weight distribution module, the weight distribution module is used to distribute the weight of the control quantity calculated by the front and rear axle PID controllers in the total control quantity;

Since the articulated vehicle is composed of two parts, the front and rear parts of the car body are usually not on the same path, and path planning for the front and rear parts of the car body is required. 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 the three reference points adjacent to the driving path. Let the path reference point be P _fi (X _fi , Y _fi ), and the two adjacent reference points are P _fi-1 (X _fi-1 , Y _fi-1 ), P _fi+1 (X _fi+1 , Y _fi+1 )

According to Figure 3, it can be known that the relationship between the steering radius R _f of the front axle of the loader and the articulation angle γ is:

_Rf = AO

AD＝l _f cosγ+l _r

R= _Rf

When the vehicle is moving forward, the path of the rear axle of the vehicle is obtained as:

X _ri ＝X _fi -l _f cosθ ₁ -l _r cos(γ-θ ₁ )

Y _ri ＝Y _fi -l _f sinθ ₁ -l _r sin(γ-θ ₁ )

The current position of the articulated vehicle is the abscissa x _f of the center point of the front axle, the ordinate y _f of the center point of the front axle, the heading angle θ _f of the front axle, and the articulation angle γ can be obtained by the navigation sensor and the rotation angle sensor, then the rear The abscissa x _r of the midpoint of the bridge, the y coordinate of the midpoint of the rear bridge y _r , and the heading angle θ _r of the rear bridge 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 -γ

According to the following formula, the distance D f between the midpoint of the front axle of the current vehicle D _f (x _f (t), y _{f (t)) and the reference path of the front axle can be calculated at the current moment t, and the minimum D f can} be found Path point, this point P _fi (X _fi , Y _fi ) is the path reference point closest to the center of the front axle:

According to calculating the path reference point closest to the center of the front axle, the path reference point P _ri (X _ri , Y _ri ) closest to the center of the rear axle can also be calculated.

Starting from the found path reference point P _fi (X _fi , Y _fi ) closest to the center of the front axle, according to the driving speed v(t) of the articulated vehicle, κ is the preview constant, and κv(t) is the preview distance, search for k points forward along the reference path as the preview point P _fi+k (X _fi+k , Y _fi+k ), as shown in Figure 4;

According to the method of calculating the preview point of the center of the front axle, the preview point P _ri+k (X _ri+k , Y _ri+k ) of the center of the rear axle can also be calculated, as shown in Figure 5;

Simplify the front axle of the vehicle into one point, take the driving direction of the front axle of the vehicle as the x-axis, and the left side of the cab as the y-axis to establish a vehicle coordinate system, as shown in Figure 6, transform the coordinates of the preview point into the vehicle coordinate system , the solution formula is as follows, and then the position error and heading error of the front axle are calculated.

y _ri+k ＝-(X _fi+k -x _f (t))sinθ _f +(Y _fi+k +y _f (t))cosθ _f

The position deviation d _f of the front axle is:

d _f =y _ri+k

Right now:

d _f ＝-(X _fi+k -x _f (t))sinθ _f +(Y _fi+k +y _f (t))cosθ _f

为：Heading deviation of the front axle

for:

According to the calculation of the deviation of the front axle, the deviation of the rear axle can also be calculated. The deviation of the rear axle of the articulated vehicle is shown in Figure 7, and the position deviation d _r is:

d _r ＝-(X _ri+k -x _r (t))sinθ _r +(Y _ri+k +y _r (t))cosθ _r

为：Heading deviation of the rear axle

for:

The calculation formula of PID controller is as follows:

The fuzzy controller is to write the corresponding fuzzy rules according to the corresponding control needs. The control idea of the articulated vehicle is as follows:

(1) When the position deviation and heading deviation are large, in order to avoid a large overshoot, K _p should take a small value as much as possible. As the articulated vehicle travels, the deviation begins to decrease, and K _p should be increased at this time. When the position deviation is small and the heading deviation is large, K _p should continue to increase, which can make the articulated vehicle return to the center in advance and reduce the overshoot of the vehicle.

(2) The function of K _i is to eliminate the stability error of the system and improve the control precision of the system, but it will also affect the vibration amplitude of the system. When the position deviation and heading deviation are relatively large, K _i takes a small value to prevent the system from oscillating greatly; when the deviation is small, increasing K _i can improve the tracking accuracy.

(3) K _d can suppress the overshoot of the system. When the position deviation and heading deviation are large, K _d takes a larger value, which can reduce the overshoot of the system; when the deviation is small, K _d should take a smaller value, then K _i can be used to a greater extent function, so that the articulated vehicle can track the reference path better.

输入到模糊PID控制器中，输出为铰接角的变化量Δδ₁。Combine the calculated position deviation d _f of the front axle with the heading deviation of the front axle

It is input into the fuzzy PID controller, and the output is the change amount Δδ ₁ of the articulation angle.

输入到模糊PID控制器中，输出为铰接角的变化量Δδ₂。Similarly, combine the calculated position deviation d _r of the rear axle with the heading deviation of the rear axle

It is input into the fuzzy PID controller, and the output is the change amount Δδ ₂ of the articulation angle.

The obtained articulation angle variation Δδ ₁ and Δδ ₂ are weighted and summed, and the variation Δδ 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 can track the upper reference path accurately and smoothly.

The above is only a specific implementation of the present disclosure, but the scope of protection of the present disclosure is not limited thereto. Anyone skilled in the art can easily think of changes or substitutions within the technical scope of the present disclosure. should fall within the protection scope of the present disclosure. Therefore, the protection scope of the present disclosure should be determined by 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 ₁ ；

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 ₂ ；

The amount of change delta of the articulation angle to be determined ₁ And delta ₂ 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.

Images