A car-train automatic parking path following algorithm

Abstract

This invention discloses an automatic parking path following algorithm for car trains. The algorithm comprises the following steps: S1: Acquire the tractor positioning signal and the trailer reference path; S2: Calculate the actual trailer pose based on the tractor positioning signal; S3: Calculate the deviation between the actual trailer pose and the trailer reference path based on the actual trailer pose; S4: Calculate the requested angle, requested speed, and requested braking command between the tractor and trailer based on the deviation between the actual trailer pose and the trailer reference path; S5: Collect the actual angle signal and actual speed between the tractor and trailer; S6: Combine the requested angle, requested speed, and requested braking command between the tractor and trailer to calculate the requested steering wheel angle, requested throttle opening, and requested braking deceleration. The automatic parking path following algorithm of this invention has clear calculation logic, few calibration parameters, and fast computation speed.

Description

Technical Field

[0001] This invention belongs to the field of autonomous driving technology, specifically relating to an automatic parking path following algorithm for car trains. Background Technology

[0002] Car trains, consisting of a tractor and trailer, are the main type of vehicle used in road transportation and are widely used in ports, industrial parks, and other similar settings, where parking operations are commonly employed. Automated parking is a crucial component of autonomous driving technology. Due to the unique structure of car trains, achieving automated parking has always been a technical challenge in the industry. Path following algorithms are the core technology of automated parking, but the effectiveness and practicality of currently available domestically developed path following algorithms for automated parking of car trains have not yet been verified. Summary of the Invention

[0003] The present invention aims to provide an automatic parking path following algorithm for automobile trains. Through improvements to the algorithm, the automatic parking path following algorithm is made clearer and faster.

[0004] The technical solution adopted in this invention is as follows:

[0005] This invention provides an automatic parking path following system and algorithm for automobile trains. The automatic parking path following system for automobile trains includes a path planning module, a combined navigation system, a tractor-trailer angle measuring device, and a chassis system. The path planning module outputs a trailer reference pose, the combined navigation system outputs a tractor reference pose, the tractor-trailer angle measuring device outputs the actual angle between the tractor and the trailer, and the chassis system outputs signals such as the actual vehicle speed.

[0006] The control flow of the automatic parking path following algorithm for car trains is as follows:

[0007] S1: Obtain the tractor positioning signal and trailer reference path;

[0008] S2: Calculate the actual position and orientation of the trailer based on the positioning signal of the tractor unit;

[0009] S3: Calculate the deviation between the actual position of the trailer and the reference path of the trailer based on the actual position of the trailer;

[0010] S4: Based on the deviation between the actual position of the trailer and the trailer reference path, calculate the requested angle, requested speed and requested braking command between the tractor and the trailer.

[0011] S5: Collect the actual angle signal between the tractor and the trailer and the actual vehicle speed;

[0012] S6: Calculate the requested steering wheel angle, requested throttle opening, and requested braking deceleration by combining the requested angle between the tractor and trailer, the requested vehicle speed, and the requested braking command.

[0013] Specifically, in S1, the tractor positioning signal includes the tractor's longitudinal coordinate x. q Horizontal coordinate y q Heading angle θ q The trailer reference path is a Bezier curve obtained based on the initial parking pose and parking space planning. The trailer reference path includes the longitudinal coordinates, lateral coordinates, and heading angle at the aiming point.

[0014] Specifically, in S2, the method for calculating the actual position and orientation of the trailer is as follows:

[0015]

[0016] In the formula, x g y g θ g These represent the trailer's longitudinal coordinates, lateral coordinates, and heading angle, x. q y q θ q These represent the longitudinal coordinates, lateral coordinates, and heading angle of the tractor unit, respectively; 'a' is the forward distance of the saddle (distance from the center of the saddle pin hole to the last axle of the tractor unit); and 'L' is the distance from the towing pin to the last axle of the trailer. This is the actual angle between the trailer and the tractor.

[0017] Specifically, in step S3, based on the obtained trailer reference pose, the deviation between the actual trailer pose and the trailer reference path is calculated. This deviation includes longitudinal deviation, lateral deviation, and heading deviation. The calculation method is as follows:

[0018]

[0019] In the formula, e x e y e θ These represent the longitudinal deviation, lateral deviation, and heading deviation between the actual position and reference path of the trailer, respectively. req y req θ req These represent the longitudinal coordinates, lateral coordinates, and heading angle of the reference path at the aiming point, respectively. g y g θ g These are the trailer's longitudinal coordinates, lateral coordinates, and heading angle, respectively.

[0020] Specifically, in S4, the calculation of the requested angle, requested speed and requested braking command between the tractor and the trailer includes two parts: lateral control and longitudinal control.

[0021] The lateral control consists of upper-level control and lower-level control. The upper-level control obtains the requested angle between the tractor and the trailer based on the deviation between the actual position and orientation of the trailer obtained in S3 and the trailer reference path through a PID algorithm.

[0022] The lower-level control of the lateral control calculates the requested steering wheel angle using the MPC algorithm based on the requested angle between the tractor and the trailer, the actual angle between the tractor and the trailer, and the vehicle speed signal.

[0023] The longitudinal control consists of upper-level control and lower-level control. The upper-level control obtains the requested speed and requested braking commands by the longitudinal deviation between the actual position of the trailer and the reference path of the trailer, the maximum speed threshold, and the braking distance.

[0024] The lower-level control of the longitudinal control combines the requested vehicle speed, the requested braking command, and the actual vehicle speed, and uses LQR and PID algorithms to derive the requested throttle opening and the requested braking deceleration.

[0025] Optionally, the deviation between the actual position of the trailer and the trailer reference path includes lateral deviation, heading deviation, curvature deviation, and longitudinal deviation.

[0026] Optionally, the PID algorithm for calculating the requested angle between the tractor and the trailer is only one possible algorithm, and other algorithms are not excluded; the MPC algorithm for calculating the requested steering wheel angle is only one possible algorithm, and other algorithms are not excluded.

[0027] Optionally, the LQR and PID algorithms for calculating the requested throttle opening and requested braking deceleration are only one option, and other algorithms are not excluded.

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

[0029] (1) The automatic parking path following algorithm of the car train of the present invention has clear calculation logic, few calibration parameters and fast calculation speed;

[0030] (2) The automatic parking path following algorithm of the present invention has been verified in real vehicles and has a high parking success rate.

[0031] (3) The automatic parking path following algorithm of the present invention has good control effect, which provides important support for the promotion and application of automatic parking technology of automobile trains. Attached Figure Description

[0032] Figure 1 This is the control flowchart of the present invention;

[0033] Figure 2 This is the overall control block diagram of the present invention;

[0034] Figure 3 This is a horizontal control block diagram of the present invention;

[0035] Figure 4 This is a vertical control block diagram of the present invention. Detailed Implementation

[0036] The present invention will be further described below with reference to the accompanying drawings and specific embodiments. It should be noted that while the description of these embodiments is intended to aid in understanding the invention, it does not constitute a limitation thereof. The functional details disclosed in this invention are only used to describe exemplary embodiments of the invention. However, the invention can be embodied in many alternative forms and should not be construed as being limited to the embodiments described herein.

[0037] It should be understood that the terminology used in this invention is only for describing specific embodiments and is not intended to limit the exemplary embodiments of the invention. When the terms "comprising," "including," "containing," and / or "comprises" are used in this invention, they specify the presence of the stated features, integers, steps, operations, units, and / or components, and do not exclude the presence or addition of one or more other features, quantities, steps, operations, units, components, and / or combinations thereof.

[0038] It should be understood that, and also noted, in some alternative embodiments, the functions / actions may appear in a different order than those shown in the figures. For example, depending on the functions / actions involved, they may actually be performed substantially concurrently, or sometimes the two figures shown consecutively may be performed in reverse order.

[0039] It should be understood that specific details are provided in the following description to facilitate a complete understanding of the exemplary embodiments. However, those skilled in the art will understand that the exemplary embodiments can be implemented without these specific details. For example, the system may be shown in block diagrams to avoid obscuring the example with unnecessary details. In other instances, well-known processes, structures, and techniques may be shown without unnecessary details to avoid obscuring the exemplary embodiments.

[0040] Example 1:

[0041] like Figure 1 As shown, an automatic parking path following algorithm for car trains includes the following steps:

[0042] S1: Obtain the tractor positioning signal and trailer reference path;

[0043] S2: Calculate the actual position and orientation of the trailer based on the positioning signal of the tractor unit;

[0044] S3: Calculate the deviation between the actual position of the trailer and the reference path of the trailer based on the actual position of the trailer;

[0045] S4: Based on the deviation between the actual position of the trailer and the trailer reference path, calculate the requested angle, requested speed and requested braking command between the tractor and the trailer.

[0046] S5: Collect the actual angle signal between the tractor and the trailer and the actual vehicle speed;

[0047] S6: Calculate the requested steering wheel angle, requested throttle opening, and requested braking deceleration by combining the requested angle between the tractor and trailer, the requested vehicle speed, and the requested braking command.

[0048] Example 2, based on Example 1, such as Figure 2 The diagram shows the overall control block diagram of the algorithm.

[0049] Specifically, the path planning module outputs the trailer reference pose, the integrated navigation system outputs the tractor pose, the tractor-trailer angle measuring device outputs the actual angle between the tractor and the trailer, and the chassis system outputs signals such as the actual vehicle speed.

[0050] In step S2, the method for calculating the actual pose of the trailer is as follows:

[0051]

[0052] In the formula, x g y g θ g These represent the trailer's longitudinal coordinates, lateral coordinates, and heading angle, x. q y q θ q These represent the longitudinal coordinates, lateral coordinates, and heading angle of the tractor unit, respectively; 'a' is the forward distance of the saddle (distance from the center of the saddle pin hole to the last axle of the tractor unit); and 'L' is the distance from the towing pin to the last axle of the trailer. This is the actual angle between the trailer and the tractor.

[0053] In step S3, based on the obtained trailer reference pose, the deviation between the actual trailer pose and the trailer reference path is calculated. This deviation includes longitudinal deviation, lateral deviation, and heading deviation. The calculation method is as follows:

[0054]

[0055] In the formula, e x e y e θ These represent the longitudinal deviation, lateral deviation, and heading deviation between the actual position and reference path of the trailer, respectively. req y req θ req These represent the longitudinal coordinates, lateral coordinates, and heading angle of the reference path at the aiming point, respectively.g y g θ g These are the trailer's longitudinal coordinates, lateral coordinates, and heading angle, respectively.

[0056] In S4, the calculation of the requested angle between the tractor and the trailer, the requested speed, and the requested braking command includes two parts: lateral control and longitudinal control.

[0057] Specifically, the algorithm is based on the control system design for trailing trailer path following. The actual pose of the trailer is derived from the pose of the tractor and the pose relationship between the trailer and the tractor.

[0058] Specifically, the algorithm consists of two parts: lateral control and longitudinal control, which control the vehicle's steering wheel angle, throttle opening, and braking deceleration, respectively.

[0059] Example 3, based on Example 2, such as Figure 3 and 4 As shown;

[0060] Specifically, the lateral control consists of upper-level control and lower-level control. The upper-level control outputs the requested angle between the tractor and the trailer based on the deviation between the actual position and the trailer reference path obtained in S3 and through the PID algorithm. The lower-level control calculates the requested steering wheel angle through the MPC algorithm based on signals such as the requested angle between the tractor and the trailer, the actual angle between the tractor and the trailer, and the vehicle speed.

[0061] Specifically, longitudinal control consists of upper-level control and lower-level control. The upper-level control takes into account signals such as the longitudinal deviation between the actual position and the trailer's reference path, the maximum speed threshold, and the braking distance, and calculates and outputs requests for speed and braking commands. The lower-level control takes into account signals such as requests for speed, requests for braking commands, and the actual speed, and calculates the requested throttle opening and requested braking deceleration through LQR and PID algorithms.

[0062] Optionally, the deviation between the actual position of the trailer and the trailer reference path includes lateral deviation, heading deviation, curvature deviation, and longitudinal deviation.

[0063] Optionally, the PID algorithm for calculating the requested angle between the tractor and the trailer is only one possible algorithm, and other algorithms are not excluded; the MPC algorithm for calculating the requested steering wheel angle is only one possible algorithm, and other algorithms are not excluded.

[0064] Optionally, the LQR and PID algorithms for calculating the requested throttle opening and requested braking deceleration are only one option, and other algorithms are not excluded.

[0065] This invention is not limited to the optional embodiments described above, and anyone can derive other various forms of products based on the inspiration of this invention. The specific embodiments described above should not be construed as limiting the scope of protection of this invention; the scope of protection of this invention should be determined by the claims, and the specification can be used to interpret the claims.

Claims

1. A car-train automatic parking path following algorithm, characterized in that, Includes the following steps: S1: Obtain the tractor positioning signal and trailer reference path; in S1, the tractor positioning signal includes the tractor's longitudinal coordinate x. q Horizontal coordinate y q Heading angle θ q The trailer reference path is a Bezier curve obtained based on the initial parking pose and parking space planning. The trailer reference path includes the longitudinal coordinates, lateral coordinates, and heading angle at the aiming point. S2: Calculate the actual position and orientation of the trailer based on the positioning signal of the tractor unit; The method for calculating the actual position of the trailer is as follows: ； In the formula, These are the trailer's longitudinal coordinates, lateral coordinates, and heading angle, respectively. These are the longitudinal coordinates, lateral coordinates, and heading angle of the tractor unit. The distance from the front of the saddle. This is the distance from the towing pin to the last axle of the trailer. This is the actual angle between the trailer and the tractor. S3: Calculate the deviation between the actual position of the trailer and the reference path of the trailer based on the actual position of the trailer; The deviation between the actual position and orientation of the trailer and the trailer reference path includes longitudinal deviation, lateral deviation, and heading deviation; the calculation method is as follows: ； In the formula, These are the longitudinal deviation, lateral deviation, and heading deviation between the actual position and orientation of the trailer and the reference path, respectively. These are the longitudinal coordinates, lateral coordinates, and heading angle of the reference path at the aiming point, respectively. These are the trailer's longitudinal coordinates, lateral coordinates, and heading angle, respectively. S4: Based on the deviation between the actual position of the trailer and the trailer reference path, calculate the requested angle, requested speed and requested braking command between the tractor and the trailer. S5: Collect the actual angle signal between the tractor and the trailer and the actual vehicle speed; S6: Calculate the requested steering wheel angle, requested throttle opening, and requested braking deceleration by combining the requested angle between the tractor and trailer, the requested vehicle speed, and the requested braking command.

2. The automatic parking path following algorithm for car trains according to claim 1, characterized in that: The S4 includes two parts: lateral control and longitudinal control. The lateral control consists of upper-level control and lower-level control. The upper-level control calculates the deviation between the actual position and the reference path of the trailer according to S3, and then obtains the requested angle between the tractor and the trailer through the PID algorithm. The lower-level control calculates the requested steering wheel angle through the requested angle between the tractor and the trailer, the actual angle between the tractor and the trailer, and the vehicle speed signal through the MPC algorithm. The longitudinal control consists of upper-level control and lower-level control. The upper-level control obtains the requested speed and requested braking command by using the longitudinal deviation between the actual position and the trailer reference path, the maximum speed threshold, and the braking distance. The lower-level control of the longitudinal control combines the requested speed, requested braking command, and actual speed to obtain the requested throttle opening and requested braking deceleration through LQR and PID algorithms.

Images