A path following device and method for a fully automatic parking system

By decoupling longitudinal and lateral control through effective distance integration and differential flatness theory, and combining it with a linear quadratic regulator, high-precision following of the automatic parking system under implicit and scattered paths is achieved. This solves the problem of decoupling longitudinal and lateral errors in existing technologies and improves vehicle following accuracy.

CN115092121BActive Publication Date: 2024-11-22TONGJI UNIV
View PDF 5 Cites 0 Cited by

Patent Information

Application Number
CN202210489943.X
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-05-06
Publication Date
2024-11-22
Estimated Expiration
2042-05-06

AI Technical Summary

Technical Problem

Existing automatic parking systems struggle to achieve high-precision path following when faced with implicit and scattered paths, and existing methods fail to effectively decouple longitudinal and lateral control errors, resulting in insufficient vehicle following accuracy.

Method used

The total reference distance is calculated using the effective distance integral method. Combining the differential flatness theory and the linear quadratic regulator, the system is projected into a linear space through feedback linearization. A longitudinal and lateral controller is designed, using arc length as the independent variable to decouple the error. The longitudinal and lateral control is achieved through driving torque and steering torque commands.

Benefits of technology

It achieves high-precision vehicle following under implicit and scattered paths, reliably eliminates longitudinal and lateral errors, is easy to control, and is suitable for various path scenarios.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN115092121B_ABST
    Figure CN115092121B_ABST
Patent Text Reader

Abstract

The present invention relates to a path following device and method for a fully automatic parking system, the device comprising: a reference point information extraction unit, used to calculate the total reference distance based on an effective distance integration method, and obtain a target vehicle speed and a reference point position; an information interaction unit, used to obtain the actual vehicle speed and the actual position of the vehicle, and to compare the target vehicle speed and the reference point position accordingly, and obtain a speed error and a position error; a chassis control unit, used to obtain the speed error and the position error, generate a corresponding driving torque command and a steering torque command, and realize longitudinal control and lateral control of the vehicle. Compared with the prior art, the present invention realizes the decoupling of lateral and longitudinal control, and further decouples the error between the vehicle and the reference point in the lateral and longitudinal directions, and controls the error by the corresponding actuator, thereby more efficiently realizing the effective elimination of the error between the vehicle and the reference point, and finally achieving the effect of enabling the vehicle to follow the path with high precision and simple design.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the field of automobile technology and driving assistance technology, and relates to a path following device and method for a full-automatic parking system. Background Art

[0002] As a representative technology of automobile intelligence, automatic parking technology has received great attention from universities and enterprises. The automatic parking command control algorithm with high-precision performance can not only make the vehicle parking without excessive safety margin, but also help improve the location planning and increase the land use efficiency, and can also be organically combined with the automatic charging technology that requires precise parking of vehicles in the future.

[0003] The current automatic parking command control algorithm can be roughly divided into two types: ① end-to-end direct output of control commands based on environmental information; ② first plan the path and then output the control command to track the path. The former is mostly heuristic algorithms such as neural networks, which are highly dependent on the performance of sensors and vehicle-related parts such as steering gears. If there is noise in the sensor information or the inertia of vehicle parts is too large, the control effect will be affected; the latter can rely on heuristic algorithms such as fuzzy logic, or closed-loop controllers such as sliding mode controllers, etc. The effect depends on the selection of feedback control state variables, such as lateral offset + angle error relative to the path, x-direction error + y-direction error + angle error relative to the reference point, etc. The control theory is relatively mature and the effect is relatively reliable.

[0004] As the scenarios of automatic parking will be more diverse in the future, the feasible parking paths are difficult to directly represent with mathematical functions in most cases, which results in the output information of path planning becoming more and more implicit paths that cannot be directly represented with mathematical functions and can only be defined with information such as shape and length, or scattered point paths represented by a series of discrete scattered points. This makes it difficult for the vehicle to directly solve the lateral offset and angle error relative to the path, and to design a separate lateral controller to follow the path. For implicit paths and scattered point paths, a relatively simple method is to set a reference point that moves along the path, and then let the vehicle follow the reference point. In this way, the position and angle error between the reference point and the vehicle can be used as the state variables of feedback control.

[0005] A good controller should be able to ensure that the vehicle can follow the path with high precision, and be simple in design and easy to use on a large scale. An effective method is to use time scale transformation to make the system use arc length as the independent variable, realize the decoupling of lateral and longitudinal control, and facilitate independent design; and feedback linearization can project the system into linear space, which is convenient for design using mature linear system control theory, and can further reduce the design difficulty. Chinese patent CN106372758B uses time scale transformation to make the system use arc length as the independent variable, so that the vehicle speed and angle control can be independently designed according to the arc length. Furthermore, for lateral control, based on the differential flatness theory, the system feedback is linearized to the differential flatness space, and a linear quadratic regulator (LQR) is used, supplemented by an α attenuation factor, to achieve effective control of the error. However, this method only realizes the decoupling of lateral and longitudinal control, and does not consider the influence of the error on the speed command and the angle command, and the accuracy is not high enough; at the same time, the fixed α attenuation factor is set throughout the process, so that the control rate cannot change adaptively with the shape of the path, and the error attenuation has not yet reached the optimal effect.

[0006] Therefore, it is of great practical significance to design a path following device for a fully automatic parking system that ensures that the vehicle can follow the path with high precision and is simple in design. Summary of the invention

[0007] The purpose of the present invention is to overcome the defects of the above-mentioned prior art and to provide a path following device and method for a fully automatic parking system, so as to enable the vehicle to follow the path with high precision while having a simple design.

[0008] The purpose of the present invention can be achieved by the following technical solutions:

[0009] A path following device for a fully automatic parking system, comprising:

[0010] Reference point information extraction unit, used to calculate the total reference distance s based on the effective distance integration method d , according to the total reference distance s d The target vehicle speed and reference point position are obtained by comparing with the planned path length;

[0011] An information exchange unit is used to obtain the actual speed and actual position of the vehicle, and compare them with the target speed and reference point position to obtain speed error and position error;

[0012] The chassis control unit is used to obtain the speed error and position error, generate corresponding driving torque instructions and steering torque instructions, and realize longitudinal control and lateral control of the vehicle.

[0013] Furthermore, the reference point information extraction unit includes:

[0014] Reference driving distance calculator, used to calculate the total reference distance s based on the actual position of the vehicle based on the effective distance integration method d Specifically, based on the reference point information of each step length, the length of the vehicle's driving path when projected onto the path is approximately calculated, and this is used as the effective distance of the step length. The effective distance of each step length is integrated to obtain the total reference distance s d ;

[0015] A vehicle speed planner is used to plan the vehicle speed according to the total reference distance s d , obtain the target speed of the reference point;

[0016] A path shape planner is used to calculate the path shape according to the total reference distance s. d , obtain the reference point position, including the coordinates and heading angle information of the reference point.

[0017] Furthermore, in the reference driving distance calculator, within each step, after obtaining the effective distance, the longitudinal error of the vehicle along the path is approximately calculated by comparing the reference point with the actual position information of the vehicle, and the effective distance of the step is corrected, and the corrected effective distance is used for the integration.

[0018] Furthermore, when the longitudinal error is greater than a set threshold, the correction is performed, and the correction is specifically:

[0019] When the vehicle lags behind the reference point, the effective distance of the step size is reduced by a multiple of σ1;

[0020] When the vehicle is ahead of the reference point, the effective distance of the step is magnified by a multiple of σ2;

[0021] Among them, σ1 and σ2 are adjustable longitudinal compensation coefficients.

[0022] Furthermore, the information interaction unit includes:

[0023] Vehicle sensors to obtain wheel speed and steering wheel angle;

[0024] The vehicle locator is used to estimate the vehicle posture according to the wheel speed and the steering wheel angle, obtain the actual speed and actual position of the vehicle, and compare them with the target speed and the reference point position to obtain the speed error and position error.

[0025] Furthermore, the chassis control unit comprises:

[0026] a vehicle speed controller, configured to receive the speed error and obtain a driving torque command based on the speed error;

[0027] a drive motor, configured to receive the drive torque instruction and generate a drive torque for longitudinal control of the vehicle according to the drive torque instruction;

[0028] A vehicle steering angle controller, configured to receive the position error and obtain a steering torque instruction according to the position error;

[0029] A steering motor, configured to receive the steering torque command and generate a steering torque for lateral control of the vehicle according to the steering torque command;

[0030] Among them, the vehicle angle controller is based on the differential flatness theory. The variables are projected from the time domain space to the differential flat space through feedback linearization. The arc length is used as the independent variable in the differential flat space through time scale transformation. In the differential flat space, a linear quadratic optimal controller is used to establish a connection between the path curvature change rate and the attenuation factor α that can accelerate the error attenuation.

[0031] The present invention also provides a path following method for a fully automatic parking system, comprising the following steps:

[0032] Calculate the total reference distance s based on the effective distance integration method d , according to the total reference distance s d The target vehicle speed and reference point position are obtained by comparing with the planned path length;

[0033] Obtaining the actual speed and actual position of the vehicle, and comparing them with the target speed and reference point position to obtain speed error and position error;

[0034] The speed error and position error are obtained, and corresponding driving torque instructions and steering torque instructions are generated to achieve longitudinal control and lateral control of the vehicle.

[0035] Furthermore, the total reference distance s is calculated based on the effective distance integration method. d Specifically include:

[0036] Get the actual position of the vehicle, loop based on the reference point information of each step, approximately calculate the length of the vehicle's driving path when projected onto the path, and use this as the effective distance of the step, integrate the effective distance of each step, and obtain the total reference distance s d .

[0037] Furthermore, within each step, after obtaining the effective distance, the longitudinal error of the vehicle along the path is approximately calculated by comparing the reference point with the actual position information of the vehicle, and the effective distance of the step is corrected, and the corrected effective distance is used for the integration.

[0038] Furthermore, when generating the steering torque command, based on the differential flatness theory, the variable is projected from the time domain space to the differential flat space through feedback linearization, the arc length is used as the independent variable in the differential flat space through time scale transformation, and a linear quadratic optimal controller is used in the differential flat space to establish a relationship between the path curvature change rate and the attenuation factor α that can accelerate the error attenuation.

[0039] Compared with the prior art, the present invention has the following beneficial effects:

[0040] (1) Reliable performance: The present invention decouples the error horizontally and vertically and corresponds it to the actuator, so that the error can be reliably kept at a small value, ultimately allowing the vehicle to follow the path with high precision;

[0041] (2) Simple design: The present invention can use arc length as an independent variable through time scale transformation, thereby achieving decoupling of lateral and longitudinal control and facilitating independent design. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 It is the overall principle block diagram of the present invention;

[0043] Figure 2 The vehicle takes the total driving distance as the total reference distance s d Schematic diagram of the vehicle following poorly;

[0044] Figure 3 It is a schematic diagram of the longitudinal error and lateral error of the vehicle;

[0045] Figure 4 It is a schematic diagram of the effective distance traveled by the vehicle in the straight section;

[0046] Figure 5 It is a schematic diagram of the effective distance traveled by the vehicle in the fixed curvature section;

[0047] Figure 6 It is a schematic diagram of the effective distance of the vehicle traveling in the variable curvature part;

[0048] Figure 7 It is a schematic diagram of a vehicle sliding at the junction of multiple paths;

[0049] Figure 8 It is a schematic diagram of the approximate calculation of the effective distance when the vehicle follows the path;

[0050] Fig. 9 It is a reference driving distance calculator design block diagram;

[0051] Fig.10 is an implicit path diagram;

[0052] Fig.11 It is a schematic diagram of implicit path recursion reference points;

[0053] Fig.12 It is a scatter path diagram;

[0054] Fig.13 It is a schematic diagram of the recursive reference points of the scattered path;

[0055] Fig.14 It is the design block diagram of the vehicle corner controller;

[0056] Fig.15 It is a schematic diagram of a vehicle traveling on a path of constant curvature with longitudinal error;

[0057] Fig.16 It is a schematic diagram of the lateral error between the vehicle and the reference point during driving;

[0058] Fig.17 It is a schematic diagram of the vehicle's operation at a variable curvature;

[0059] Fig.18 It is a schematic diagram of the vehicle's vertical parking path following effect;

[0060] Fig.19 It is the error data in the x and y directions between the midpoint of the vehicle's rear axle and the reference point when the vehicle follows the perpendicular parking path;

[0061] Fig. 20 It is a schematic diagram of the path following effect of parallel parking of vehicles;

[0062] Fig.21 It is the error data in the x and y directions between the midpoint of the vehicle's rear axle and the reference point when the vehicle follows the parallel parking path. DETAILED DESCRIPTION

[0063] The present invention is described in detail below in conjunction with the accompanying drawings and specific embodiments. This embodiment is implemented based on the technical solution of the present invention, and provides a detailed implementation method and specific operation process, but the protection scope of the present invention is not limited to the following embodiments.

[0064] The present invention proposes a path following device for a fully automatic parking system, such as Figure 1 As shown, the whole device includes three parts: a reference point information extraction unit, an information interaction unit and a chassis control unit.

[0065] As mentioned in the background technology part of the present invention, in the future automatic parking algorithm, the results of path planning will be mainly in the form of implicit path or scattered path output. For these two forms, a relatively simple method is to set a reference point moving along the path, and then let the vehicle follow the reference point. In this way, the error between the vehicle and the reference point will become an important feedback quantity to achieve feedback control. Chinese patent CN106372758B discloses a path following method and device for an assisted parking system. Although it can achieve a certain accuracy, there are still errors in the information feedback process in actual use. The inventor of this application found through creative research that the main reason for the incorrect information feedback is that patent CN106372758B only uses the total driving distance of the vehicle as the total reference distance s. d , and the error decoupling cannot be performed. Figure 2 This is a common scenario when a vehicle follows a path. The midpoint of the vehicle's rear axle starts from point P and moves to point O. At this time, the arc length The reference point is O r . O point and O r The error between the points can be divided into the lateral deviation error OQ of the vehicle relative to the path and the longitudinal error O r Q. The impact is:

[0066] ① In longitudinal control, the expected speed v d It should be ensured that the vehicle stops after the entire journey is completed. Therefore, when designing, the total reference travel distance s d When the total length l of the planned path is reached, v d is designed to be 0. At this time, if s d If the calculation is improper, there will be Figure 3 The longitudinal error along the path shown on the left is when the reference point O r Arriving at the destination causes the vehicle speed command value to be 0, causing the vehicle to stop early and the midpoint O of the vehicle's rear axle unable to reach the destination.

[0067] ② In lateral control, the midpoint O of the vehicle's rear axle and the reference point O r The error between the midpoint of the vehicle rear axle and the reference point will be fed back to the lateral controller for lateral control. Figure 3 As shown in the figure, the longitudinal error and the lateral error are two parts, the lateral control will also receive the longitudinal error information that cannot be controlled by it, and the longitudinal error will instead affect the lateral control, which will be explained later.

[0068] It can be seen that d Improper calculation will affect both the horizontal and vertical control, but through the design of reasonable calculation s dThe method can eliminate the longitudinal error and achieve good longitudinal control; after the longitudinal error is eliminated, the lateral controller is designed to eliminate the remaining lateral error, and finally the best performance can be achieved. In the device of the present invention, the longitudinal control is implemented in the reference driving distance calculator of the reference point information extraction unit, and the lateral control can be implemented in the vehicle turning angle controller of the chassis control unit. The information interaction unit is responsible for the information interaction between the reference point information extraction unit and the chassis control unit in the whole device. Based on the above analysis, the three units involved in the present invention are now introduced in detail.

[0069] 1. Reference point information extraction unit

[0070] The main function of the reference point information extraction unit is to receive vehicle positioning information in real time during vehicle operation and output path reference point information, so that the information interaction unit can compare the vehicle state with the reference point state and send the error to the chassis control unit for chassis control. The unit's reference point extraction method can adapt to different path planning output results, such as implicit paths planned based on CC curves that cannot be directly represented by mathematical functions and can only be defined by shape and length, or a series of discrete scattered point paths planned by machine learning methods. The specific functions of this unit are: ① Perform longitudinal control and eliminate longitudinal errors; ② Obtain reference point information.

[0071] (1) Longitudinal control design

[0072] As mentioned in the previous analysis, the reason why the vehicle introduces longitudinal error is the total reference distance s d The calculation of the total reference distance s is incorrect, so it is necessary to design d The present invention adopts the effective distance integration method to calculate s d Calculation.

[0073] The effective distance integration method uses the coordinates of the reference point of each step, the heading angle and other information to approximately calculate the length of the vehicle's driving path after projection onto the path, and uses this as the effective distance and integrates it to obtain the effective reference driving distance s. d , to the greatest extent possible to avoid the invalid distance being counted into s when the vehicle deviates from the path due to poor following d The specific process is as follows:

[0074] Although the vehicle is always moving forward, not all the distance it has traveled is along the path. Figure 2 For example, although the vehicle starts from point P and runs to point O, in the direction of the path, the vehicle actually only travels to point Q. If the total distance traveled by the vehicle, that is, the length of the arc length PO, is directly used as the total reference travel distance s d , which will result in the reference point being located at O rThe position is ahead of point Q, which leads to a longitudinal error.

[0075] To address the above problems, the effective distance integration method is introduced. The effective distance is defined as the length of the vehicle's driving path projected onto the planned path. Since the vehicle will travel on straight lines, constant curvature arcs, and variable curvature curves, the corresponding effective distances in the three cases are defined as follows:

[0076] A. Straight section: The effective distance is the direct projection length CD of the actual driving path OO′ on the straight line, such as Figure 4 As shown;

[0077] B. Constant curvature arc part: When the vehicle is driving, the effective angle of the vehicle relative to the arc can be calculated by using the coordinates of the starting point O and the end point O' of the midpoint of the rear axle of the vehicle. The effective distance is the arc length CD corresponding to this part of the angle, such as Figure 5 As shown;

[0078] C. Variable curvature curve part: Since this part of the effective distance is difficult to calculate accurately, when calculating, the curvature corresponding to the starting point O and the end point O' is used to calculate the corresponding lengths under the front and rear curvatures according to the calculation method in B, and then the average value of the two is taken to approximate the effective distance CD, such as Figure 6 shown.

[0079] In this way, every time the vehicle runs for a time interval, the distance traveled by the vehicle in this time interval is corrected according to the above rules to obtain the effective distance Δs in this time interval. d , and use such Δs d Integrate to obtain the total length s d .

[0080] However, sometimes, simply integrating the effective distance is not enough to achieve optimal performance, because the calculation of the effective distance integral will be affected by accidental errors, resulting in calculation deviations. Figure 7 As shown, when the vehicle performs multi-stage parking, after following the first path BA, there is a slipping situation at the segment change junction point A, which causes the vehicle to have an initial longitudinal error OA indicated by the arrow when following the next segment AC, and this part cannot be compensated by the above-mentioned integral mechanism. In a preferred embodiment, a correction method for the longitudinal integral needs to be designed to eliminate this part of the error.

[0081] Longitudinal integral correction method: In each step, by comparing the coordinates, heading angle and other information of the reference point with the vehicle, the longitudinal error of the vehicle along the path is approximately calculated, and then the effective distance is corrected and integrated to eliminate the cumulative longitudinal error caused by the approximate calculation of the effective distance when the vehicle is driving, as well as the longitudinal error introduced by the vehicle slipping when the vehicle is driving in multiple sections. The specific process is as follows:

[0082] When there is an initial longitudinal error, the most intuitive idea is that when the vehicle lags behind the reference point, we hope that the reference point moves less so that the vehicle can catch up with the reference point; conversely, we hope that the reference point moves more so that the error between the vehicle and the reference point can be reduced. Based on this idea, a longitudinal integral correction method can be set. d After that, the following correction is required to be included in the total length s d :

[0083] A) When the vehicle lags behind the reference point: it is hoped that the reference point does not move too much, and the effective distance included in this calculation is reduced by a multiple of σ1;

[0084] B) When the vehicle is ahead of the reference point: it is hoped that more distance will be included so that the reference point catches up with the vehicle. The effective distance included this time will be magnified by a multiple of σ2.

[0085] σ1 and σ2 are adjustable longitudinal compensation coefficients. When compensating, the leading and lagging of the vehicle need to calculate the longitudinal error along the path and judge it in combination with the vehicle's driving direction. At the same time, to avoid the system being too sensitive, this correction should only be performed after the longitudinal error exceeds a certain threshold, which can be set to a scale value of a code disk.

[0086] Since there is only information about one reference point, the longitudinal error required for longitudinal compensation cannot actually be accurately calculated, and the longitudinal error can only be approximately calculated by using the information of the reference point.

[0087] The above-mentioned effective distance integration method and longitudinal integration correction method can be built into the reference driving distance calculator to realize the corresponding functions.

[0088] Figure 8 This shows a common scenario of the following process, where the midpoint O of the vehicle's rear axle is aligned with the reference point O. r There are both lateral and longitudinal errors between them. Obviously, the most ideal lateral reference point is point S, while point O r The distance between O and S is the longitudinal error. r The information of O cannot be used to calculate the path length between the two. r The coordinates, heading angle, curvature, and turning direction of O are all known, so r The circle R that is tangent to its heading angle is uniquely determined.r The arc length of the circle R corresponding to R and OR is considered to be the approximate longitudinal error, and its positive or negative value is determined by the vehicle's forward direction. In actual operation, the longitudinal error is calculated once every time the data is updated. If it exceeds the threshold, the effective distance Δs at that moment is d Make corrections and include in total length s d Since the longitudinal error cannot obtain an accurate value, it actually serves as a "warning" for longitudinal compensation.

[0089] At the same time, if Figure 1 As shown, since the lateral and longitudinal controls are based on the same effective distance s d , if there is a longitudinal error during actual following, the lateral controller will still be affected by the longitudinal error and cannot follow the path well before the longitudinal error is compensated, introducing additional lateral errors. However, under low-speed parking conditions, the longitudinal error that can be introduced by approximate calculation is very small. At the same time, even if the car slips, the longitudinal error introduced is only about 10 centimeters. Therefore, the longitudinal error can be compensated in a very short time after the real-time data is updated three to four times. After the compensation is completed, the lateral controller can eliminate the lateral error introduced when the longitudinal error is not compensated according to the corresponding design.

[0090] The overall block diagram of longitudinal control is as follows: Fig. 9 shown.

[0091] (2) Reference point information acquisition

[0092] The vehicle speed planner and path shape planner have stored the output results of path planning before the vehicle runs, and then they can combine the planned path length with the total reference distance s d The vehicle speed planner is used to calculate the vehicle speed according to the total reference distance s. d , obtain the target speed of the reference point; the path shape planner is used to calculate the total reference distance s d , obtain the reference point position, including the coordinates and heading angle information of the reference point.

[0093] As mentioned in the background technology section of the present invention, in the future automatic parking algorithm, the path planning results will be mainly in the form of implicit path or scattered path output. The reference point acquisition methods corresponding to the two planning output forms are slightly different, which are described in detail below.

[0094] ① The planning result is an implicit path

[0095] Implicit paths such as Fig.10 As shown. The output result of the implicit path can be regarded as a series of straight line segments from A1 to A6 and the CC curve. The expression form of the planning result is a segmented implicit form as shown in formula (1.1).

[0096]

[0097] Where l1, l2, and l3 are segmented boundaries along the arc length, which can be understood as Fig.10 The arc lengths between A1A2, A1A3, etc.; ρ1, ρ2, ρ3 are the curvature values ​​in this segment; q1, q2, q3 are the vehicle driving directions in this segment (forward is 1, reverse is -1); π1, π2, π3 are the steering directions of the steering wheel in this segment (left turn is 1, right turn is -1); s d is the length along the path.

[0098] The calculation of the reference point coordinates and heading angle is obtained recursively based on the geometric shape. The total reference distance, that is, the length along the path s d , compared with the segment boundary values ​​l1, l2, l3, etc., to obtain the reference point O at this time r1 The curvature ρ, driving direction u and turning direction π. If the reference point coordinates and heading angle are x d ,y d ,θ d , the length of the path increases by Δs d , then Fig.11 , recursively calculate the reference point O at the next moment r2 Coordinates and heading angle x′ d , y′ d ,θ′ d for

[0099]

[0100] The vehicle speed planner stores the target speed v d With the length s along the path d , the function v related to the total path length l d =f(s d ,l),s d After obtaining it, you can directly obtain the expected speed.

[0101] ② The planning result is a scattered path

[0102] The scatter path is as follows Fig.12 As shown in . The scattered point path is a path composed of a series of scattered points that are related to each other. The scattered points can be simple coordinate points or trajectory points containing speed. The recursive method is as follows Fig.13 As shown. Scatter B k With B k+1 The space between them can be approximated as a small arc, and the current reference point is O r1 And with B k+1 The distance is Δs d1, and if the reference driving distance calculator calculates that the reference point should move forward Δs d , then the reference point will cross B k+1 and in B k+1 With B k+2 Between, and the distance spanned is Δs d2 =Δs d -Δs d1 . Because B k+1 With B k+2 If the information of is known, we can get the k+1 B k+2 On, and with B k+1 Distance Δs d2 The coordinate point information of the location can be regarded as the reference point.

[0103] If the planning result is a trajectory point sequence, the expected speed v d It can be obtained by interpolating the speed of the two trajectory points on both sides of the reference point; if the planning result is a simple discrete coordinate point, the entire length of the path can be calculated after the planning is completed, and the vehicle speed v can be designed. d With the length s along the path d , the function v related to the total path length l d =f(s d ,l), and then according to the actual s d Get the expected speed.

[0104] 2. Chassis control unit

[0105] The main function of the chassis control unit is to receive the speed error between the actual speed of the vehicle and the expected speed, the position error between the midpoint of the rear axle of the vehicle and the reference point, and output the driving torque command and steering torque command to the driving motor and steering motor, and finally output the driving torque and steering torque to directly control the operation of the vehicle. The main functions of this unit are: ① Perform lateral control to eliminate lateral errors; ② Adjust the actual speed of the vehicle to the expected speed.

[0106] (1) Lateral control design

[0107] A. Vehicle Kinematic Model

[0108] Since the vehicle speed is very low during parking, the influence of the vehicle sideslip angle is ignored and the vehicle kinematic model is used:

[0109]

[0110] Where x(t) and y(t) are the coordinates of the midpoint of the rear axle of the vehicle, θ(t) is the heading angle of the vehicle, and v(t) is the speed of the midpoint of the rear axle. is the front wheel turning angle, and L is the wheelbase.

[0111] Furthermore, the vehicle's steering mechanism can also be simplified to an inertia link:

[0112]

[0113] Among them, u1(t) is the input of the actuator, and T is the time constant of the inertia link.

[0114] As can be seen from the above two equations, the kinematic equation contains the time variable velocity v(t), which is not conducive to the independent design of velocity and angle when designing the subsequent control instructions. Therefore, the arc length is first introduced as a new independent variable to achieve horizontal and vertical decoupling. At the same time, in order to facilitate the subsequent feedback linearization processing, the time scale function u2(t) is introduced:

[0115]

[0116] s d That is, the total travel distance along the path; u2(t) can actually be understood as the difference between the actual travel distance ds of the vehicle and the travel distance ds along the path in a small time dt. d The ratio of , and its use for feedback linearization will be described later. Obviously, if the vehicle can completely follow the path, it means that the total distance traveled by the vehicle is equal to the distance traveled along the path, and u2(t) = 1. In this way, by using the chain rule and formula (2.3) to process formula (2.1) and (2.2), x(t), y(t), θ(t), All are transformed into relationships with arc length as the independent variable:

[0117]

[0118] at the same time

[0119]

[0120] It should be noted here that the velocity v contained in equation (2.4) is v(s d ).

[0121] B. Feedback Linearization

[0122] Equation (2.4) is the kinematic model of the vehicle with respect to arc length. It is still a nonlinear model, which is not convenient for the subsequent processing of state equations and the design of feedback control. Therefore, a feedback linearization process is required.

[0123] In the differential flat space, the arc length is still used as the independent variable, and the differential flat output is set to the horizontal and vertical coordinates x(s d )、y(s d ). The proof of the differential flatness of the system is omitted here.

[0124] By arranging, the variables involved in the system can be arranged into differentially flat output elements themselves and their finite-order derivative forms:

[0125]

[0126] The arrangement of the state equation also requires the corresponding values ​​at the ideal point. Due to the reference point recursive relationship mentioned above, the reference point coordinate x r ,y r , reference point processing heading angle θ r , the curvature ρ at the reference point r Can be obtained directly.

[0127] but

[0128]

[0129]

[0130] By adding x r Arc length s d Taking the derivative, we can calculate the x′ required for feedback control r , x″′ r , x″′ r for

[0131]

[0132] y r and its derivatives and other calculations with x r etc.

[0133] According to formula (2.4), the sum of the squares of formula one and formula two is

[0134]

[0135] Taking the derivative twice gives

[0136]

[0137] At the same time, using formula (2.4) Arrange and substitute The correlation is

[0138]

[0139] Formula (2.6) uses vehicle-related variables to express the calculation The required x, x′, x″, y, y′, y″, and feedback control is actually designed by calculating x″′, y″′. The last two points are obtained Can be matched Calculate the actual command value y1. Here we can see that the introduction of the time scale function u2 provides a channel for the projection of vehicle-related variables into the differential flat space and the projection of the command back into the time domain space.

[0140] Note δ x =xx r In the control process, it is hoped that the error will decay toward 0. To facilitate the subsequent arrangement of the state equation, taking the x direction as an example, the following equation is constructed:

[0141] δ″′ x +k3δ″ x +k2δ′ x +k1δ x =0 (2.13)

[0142] The above equation is a third-order linear differential equation, which can be solved for δ x =δ x (s d ). By designing k1, k2, and k3, δ x When the initial value is not 0, it decays toward 0 to achieve error stabilization.

[0143] Expand equation (2.13), let w1 = x″′, w2 = y″′, we have

[0144]

[0145] Treating w1 and w2 as x″′ and y″′ at all times means that equation (2.13) always holds true, which will cause the error to decay toward 0. By using w1 and w2 to replace the x″′ and y″′ pairs in equations (2.11) and (2.12), Calculation is performed to finally realize the transformation from error to front wheel steering angle control value. It should be noted here that since the derivative of arc length is calculated in differential flat space, the derivative representation is different from equation (2.1).

[0146] Based on the above description, the overall design block diagram of the lateral control implemented by the vehicle corner controller is as follows: Fig.14 shown.

[0147] Here we briefly explain the influence of the longitudinal error on the lateral direction mentioned above. Fig.15 As shown in the figure, the vehicle is traveling on an arc with a constant curvature and a radius of R. According to the Ackerman steering model, the front wheel steering angle of the vehicle is kept at

[0148]

[0149] to follow the path exactly.

[0150] If there is a longitudinal error OO r, then substitute equations (2.6) (2.14) into (2.10) (2.12) and sort them out to find that for the instruction u1(t), it not only includes the It also contains additional x , δ y ,θ-θ r The relevant additional amount, while δ x , δ y ,θ-θ r When the radius R is constant and the longitudinal error OO r The result is that the front wheel steering angle command cannot be maintained. And deviate from the path, and if the longitudinal error OO r The larger it is, the greater the impact.

[0151] C. Feedback Control Description

[0152] In the previous section, we mentioned that we use the designed w1 and w2 to replace x″′ and y″′ for calculation. In fact, it is a real variable, which has an error with the designed w1 and w2, which can be expressed as

[0153]

[0154] Taking the x-direction error as an example, let δ x =xx r , according to formula (2.13) (2.16), with δ x ,δ′ x ,δ″ x is the state variable, and the state equation is

[0155]

[0156]

[0157] In fact, if the vehicle can follow the path completely, Δ x =0. This can be proved by substituting x″ in equation (2.6) and taking its derivative when the path is completely followed, and comparing it with equation (2.9). At the same time, according to the previous formula derivation, it can be seen that the calculation of w1 and w2 involves δ x ,δ′ x ,δ″ x The product of k1, k2, k3, and the actual x″′, y″′ is calculated by (2.6) without designing the above quantities, that is, δ x ,δ′ x ,δ″ x If the product of k1, k2, and k3 is too large, it may lead to Δ x Too large, the control design is inaccurate, so when the error δx ,δ′ x ,δ″ x When k1, k2, and k3 are small, Δ x It may be small and negligible.

[0158] In subsequent design, it is assumed that the above conditions are met and the influence of this item is ignored. y The state equation is the same as equations (2.17) and (2.18), so if δ x , δ y The state equation of is treated in the same way, then δ x , δ y The decay rate is the same.

[0159] The above state equation is arranged to facilitate the determination of the feedback coefficients k1, k2, k3 in equation (2.13), thereby determining the error term δ x , δ y and the decay rate of its derivatives of various orders. Chinese patent CN106372758B points out that the decay rate of the error depends on the dominant pole of the system. Therefore, on the basis of using LQR to determine the feedback coefficient, the decay factor α is introduced. The principle is that for the state equation

[0160]

[0161] When using LQR calculation, the matrix A+αI is used instead of the matrix A for calculation, which makes the dominant pole of the system farther away from the imaginary axis, so that the error decays at a faster rate. Here I is the unit matrix.

[0162] D. Error consistency description

[0163] The lateral error between the path itself and the vehicle is reflected in the lateral offset and angle error, and the control rate of the present invention is for δ x , δ y and its derivative, therefore, it is necessary to explain the unity of the above errors, thus proving that x , δ y The design of the attenuation rate is effective. The error diagram is as follows Fig.16 As shown in Figure 2. Where l is the lateral offset.

[0164] For the lateral offset l, it is obvious that

[0165]

[0166] Of course, if δ x , δ y Index The speed decays,

[0167]

[0168] For the angle deviation θ-θ r , combined with Compared with the small angle assumption (θ-θ r Less than 5°),

[0169]

[0170] therefore

[0171]

[0172] Of course, if δ′ x ,δ′ y Index The speed decays,

[0173]

[0174] Equations (2.20) to (2.24) show that the lateral offset and the angle error and δ x , δ y There is a corresponding relationship between δ x , δ y The design of equal attenuation rate is equivalent to the design of attenuation rate for lateral offset and angle error.

[0175] E. Feedback Control Design

[0176] Since the initial error of the vehicle is 0, but an error occurs during the following process, it means that the error is introduced by the change in the shape of the path. Since the shape of the path changes continuously, the introduction of the error is also a smooth process. Therefore, if the law of error introduction can be studied, the error can be controlled to a small value throughout the parking process in combination with the law of error introduction.

[0177] If the error is regarded as a function f(s d ), then according to the rectangular integral formula, according to Δs d The step size is discretized and combined with the step function, it can be rewritten as

[0178]

[0179] u(0), u(Δs d ) is a step function. The above formula shows that the error can be regarded as the successive superposition of a series of step function values, and its numerical correctness can be verified by substituting s d =Δs d 、s d =2Δs d Therefore, if we can obtain f′(0), f′(Δs d), etc., the control rate can be designed to eliminate the errors as they just begin to accumulate, so that the errors will not accumulate to a large value.

[0180] When the front wheel angle of the vehicle is kept fixed, the vehicle's running trajectory is a fixed arc. If the planned path is a constant curvature arc, the vehicle will not continue to have lateral errors after the front wheel angle is stable. Therefore, the accumulation of errors occurs when the curvature changes.

[0181] Fig.17 The schematic diagram of the vehicle running at the variable curvature. When the vehicle is at point A, the vehicle is close to the reference point O r Due to the lack of error feedback, the vehicle maintains the arc path to point B. However, due to the change in curvature, the vehicle moves forward Δs d The actual rear reference point should be O' r This creates a lateral error in the system, and the accumulation of this error will eventually manifest itself as the vehicle deviating from the path.

[0182] Fig.17 BO′ r The distance between them is the forward movement Δs d Lateral offset introduced, B and O′ r The heading angle deviation between is the introduced angle error. Let the radii of circles R1 and R2 be r1 and r2, and the curvatures be ρ1 and ρ2, then

[0183]

[0184]

[0185] Using the law of cosines, we have BR2 length

[0186]

[0187] but

[0188]

[0189]

[0190] Considering the square root in the denominator same Compared with much smaller, formula (2.29) can be further rearranged into

[0191]

[0192] Here ρ′ is the curvature change rate. Equation (2.30) shows that after a fixed Δs d length, the lateral offset introduced into the system by the curvature change rate ρ′ It is approximately in direct proportion to ρ′ itself.

[0193] Similarly, the angle error introduced by ρ′ is

[0194]

[0195] It can be seen that the introduced angle error σ is also approximately in direct proportional function with the curvature change rate ρ′.

[0196] As described in Chinese patent CN106372758B, if the real part of the dominant pole of the system is -μ, the error is If we assume that the system has an initial error τ(0), after Δs d After that, the error becomes

[0197]

[0198] Taking the lateral offset as an example, if the curvature change rate is ρ′, then combined with the intervening lateral offset error, Δs d After that, the error becomes

[0199]

[0200] If the attenuation factor α=β1ρ′ related to the curvature change rate is added at this time, then

[0201]

[0202] From the second part of equation (2.34), it can be seen that if the parameter β1 can be adjusted appropriately, after the error accumulates to a certain value, the decay rate increased by the decay factor α can completely offset the error introduced subsequently, that is, the error will be maintained at a certain upper limit value τ(0) and will not continue to increase. At the same time, in order to maintain a sufficient error decay rate when the vehicle is traveling on a constant curvature path, α can be set to β1ρ′+β2, where β1 and β2 are adjustable parameters determined by the actual tracking effect of the vehicle.

[0203] It is explained here that in actual operation, the upper limit of the error τ(0) cannot be accurately determined by β1 according to formula (2.34). This is because the addition of the α factor is not strict, which makes the dominant pole farther away from the imaginary axis α; at the same time, although the longitudinal error can be kept at a small value, it cannot be completely eliminated to 0, and the residual longitudinal error will still affect the lateral control; the approximation in the derivation will also cause a slight impact.

[0204] (2) Vehicle speed control

[0205] Vehicle speed control can be achieved using traditional methods such as PID control without any specific requirements.

[0206] In one specific embodiment, the chassis control unit includes:

[0207] a vehicle speed controller, configured to receive the speed error and obtain a driving torque command based on the speed error;

[0208] a drive motor, configured to receive the drive torque instruction and generate a drive torque for longitudinal control of the vehicle according to the drive torque instruction;

[0209] A vehicle steering angle controller, configured to receive the position error and obtain a steering torque instruction according to the position error;

[0210] The steering motor is used to receive the steering torque instruction and generate a steering torque for lateral control of the vehicle according to the steering torque instruction.

[0211] Among them, the vehicle angle controller is based on the differential flatness theory. The variables are projected from the time domain space to the differential flat space through feedback linearization. The arc length is used as the independent variable in the differential flat space through time scale transformation. In the differential flat space, a linear quadratic optimal controller (LQR) is used to establish a connection between the path curvature change rate and the attenuation factor α that can accelerate the error attenuation, ensuring that the vehicle can avoid lateral error accumulation when the curvature changes, thereby following the reference point with high precision, and then following the path with high precision.

[0212] The error term after feedback linearization can establish an equation relationship with the lateral offset and angular error of the vehicle relative to the path when there is no feedback linearization. This ensures that the design of the error control rate in the differential flat space is equivalent to the design of the error control rate of the lateral offset and angular error of the vehicle relative to the path in the time domain space.

[0213] If the longitudinal error is not completely eliminated, after the error between the vehicle and the reference point is sent to the vehicle angle controller, an interference term proportional to the longitudinal error will appear, affecting the vehicle's following of the path, which indirectly illustrates the necessity of longitudinal error compensation. When the longitudinal error between the vehicle and the reference point is not completely compensated, the longitudinal error still affects the vehicle angle and introduces additional lateral errors. After the longitudinal error is compensated, the vehicle angle controller can eliminate the additional lateral errors introduced when the longitudinal error is not fully compensated according to the designed control rate.

[0214] 3. Information interaction unit

[0215] The main function of the information interaction unit is to use various on-board sensors to extract the vehicle wheel speed and steering wheel angle when the vehicle is running and send them to the vehicle positioning module, which converts the wheel speed into the actual vehicle speed, estimates the posture to obtain the actual position, and sends the vehicle running status to the reference point information extraction unit to facilitate the update of the reference point, and use the deviation for subsequent control. This part is highly dependent on hardware, so it is not introduced in detail in this invention.

[0216] The actual vehicle effect of this device is as follows Figures 18 to 21 The actual vehicle effect shows that the device can achieve high-precision vehicle tracking, and the errors in the x and y directions can be kept within 5 cm.

[0217] The above-mentioned device realizes the decoupling of lateral and longitudinal control through time scale transformation and takes arc length as the independent variable, which facilitates the independent design of the two; at the same time, the lateral and longitudinal errors are further decoupled and controlled by the corresponding actuators, which more efficiently realizes the effective elimination of the error between the vehicle and the reference point, and ultimately enables the vehicle to follow the path with higher accuracy.

[0218] Based on the reference point information extraction unit, the information interaction unit and the chassis control unit, a path following method for a fully automatic parking system can be implemented, including the following steps: calculating the total reference distance s based on the effective distance integration method d , according to the total reference distance s d The target vehicle speed and the reference point position are obtained by comparing the target vehicle speed and the planned path length; the actual vehicle speed and the actual position of the vehicle are obtained, and the speed error and the position error are obtained by comparing the target vehicle speed and the reference point position; the speed error and the position error are obtained, and the corresponding driving torque command and the steering torque command are generated to realize the longitudinal control and the lateral control of the vehicle.

[0219] The above method can also be implemented in the form of a software functional unit and can be stored in a computer-readable storage medium when it is sold or used as an independent product. Based on this understanding, the technical solution of the present invention can be essentially or partly embodied in the form of a software product that contributes to the prior art. The computer software product is stored in a storage medium and includes several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0220] The preferred specific embodiments of the present invention are described in detail above. It should be understood that a person skilled in the art can make many modifications and changes based on the concept of the present invention without creative work. Therefore, any technical solution that can be obtained by a person skilled in the art through logical analysis, reasoning or limited experiments based on the concept of the present invention on the basis of the prior art should be within the scope of protection determined by the claims.

Claims

1. A path following device for a fully automatic parking system, characterized in that: include: Reference point information extraction unit, used to calculate the total reference distance based on the effective distance integration method , according to the total reference distance The target vehicle speed and reference point position are obtained by comparing with the planned path length; An information exchange unit is used to obtain the actual speed and actual position of the vehicle, and compare them with the target speed and reference point position to obtain speed error and position error; A chassis control unit, used to obtain the speed error and position error, generate corresponding driving torque instructions and steering torque instructions, and realize longitudinal control and lateral control of the vehicle; The reference point information extraction unit comprises: Reference driving distance calculator, used to calculate the total reference distance based on the effective distance integration method according to the actual position of the vehicle Specifically, the reference point information of each step is cyclically calculated, and the length of the vehicle's driving path is approximately calculated after the length of the path is projected, which is used as the effective distance of the step, and the effective distance of each step is integrated to obtain the total reference distance. ; The vehicle speed planner is used to calculate the vehicle speed according to the total reference distance. , obtain the target speed of the reference point; A path shape planner is used to calculate the path shape according to the total reference distance. , obtain the reference point position, including the coordinates and heading angle information of the reference point; In the reference driving distance calculator, after obtaining the effective distance in each step, the longitudinal error of the vehicle along the path is approximately calculated by comparing the reference point with the actual position information of the vehicle, and the effective distance of the step is corrected, and the corrected effective distance is used for the integration; When the longitudinal error is greater than a set threshold, the correction is performed, and the correction is specifically: When the vehicle lags behind the reference point, the effective distance of the step length is The multiple of is reduced; When the vehicle is ahead of the reference point, the effective distance of the step length is The multiple of magnification; in, , Both are adjustable longitudinal compensation coefficients; The chassis control unit comprises: a vehicle speed controller, configured to receive the speed error and obtain a driving torque command based on the speed error; a drive motor, configured to receive the drive torque instruction and generate a drive torque for longitudinal control of the vehicle according to the drive torque instruction; A vehicle steering angle controller, configured to receive the position error and obtain a steering torque instruction according to the position error; A steering motor, configured to receive the steering torque command and generate a steering torque for lateral control of the vehicle according to the steering torque command; The vehicle steering controller is based on the differential flatness theory. It projects the variables from the time domain space to the differential flat space through feedback linearization. The arc length is used as the independent variable in the differential flat space through time scale transformation. In the differential flat space, the linear quadratic optimal controller is used to convert the path curvature change rate and the attenuation factor that can accelerate the error attenuation. Make connections.

2. The path following device of the fully automatic parking system according to claim 1, characterized in that: The information interaction unit comprises: Vehicle sensors to obtain wheel speed and steering wheel angle; The vehicle locator is used to estimate the vehicle posture according to the wheel speed and the steering wheel angle, obtain the actual speed and actual position of the vehicle, and compare them with the target speed and the reference point position to obtain the speed error and position error.

3. A path following method for a fully automatic parking system, characterized in that: The following steps are involved: Calculate the total reference distance based on the effective distance integration method , according to the total reference distance The target vehicle speed and reference point position are obtained by comparing with the planned path length; Obtaining the actual speed and actual position of the vehicle, and comparing them with the target speed and reference point position to obtain speed error and position error; Obtaining the speed error and position error, generating corresponding driving torque command and steering torque command, and realizing longitudinal control and lateral control of the vehicle; The total reference distance is calculated based on the effective distance integration method Specifically include: Get the actual position of the vehicle, loop based on the reference point information of each step, approximately calculate the length of the vehicle's driving path when projected onto the path, use this as the effective distance of the step, integrate the effective distance of each step, and obtain the total reference distance ; In each step, after obtaining the effective distance, the longitudinal error of the vehicle along the path is approximately calculated by comparing the reference point with the actual position information of the vehicle, and the effective distance of the step is corrected, and the corrected effective distance is used for the integration; When generating the steering torque command, based on the differential flatness theory, the variable is projected from the time domain space to the differential flat space through feedback linearization, the arc length is used as the independent variable in the differential flat space through time scale transformation, and the path curvature change rate and the attenuation factor that can accelerate the error attenuation are converted into the linear quadratic optimal controller in the differential flat space. Make connections.

Citation Information

Patent Citations

  • A path following method and apparatus for an assisted parking system

    CN106372758B

  • State feedback based vehicle path tracking H-infinity control method

    CN107015477A

  • Unmanned vehicle, terminal point lateral steady-state control method thereof and electronic equipment

    CN112124295A

  • Parameter self-tuning LQR path tracking method with PID corner compensation

    CN113608530A

  • Parking remaining distance calculation method and system, longitudinal control method and system and vehicle

    CN114132306A