A reverse tracking control method and system based on trajectory tracking

Abstract

The application discloses a kind of based on trajectory tracking's reverse tracking control method and system, comprising: positive trajectory storage stage and reverse tracking stage;Positive trajectory storage stage includes: real-time acquisition vehicle attitude coordinates and driving mileage;Store vehicle position coordinates;Reverse tracking stage includes: calculate tracking total mileage;Determine final tracking mileage;Tracking remaining driving mileage;Obtain initial target trajectory point;Determine final target trajectory point;Calculate steering wheel angle control value;Whether remaining driving mileage is less than preset distance is judged, if yes, end reverse tracking;If no, real-time acquisition vehicle attitude coordinates and driving mileage, according to driving mileage and final tracking mileage, calculate remaining driving mileage, and track remaining driving mileage.The application can realize closed-loop trajectory tracking when reversing by storing vehicle attitude coordinates of positive driving, and can avoid the problem of deviation from trajectory when reversing.

Description

Technical Field

[0001] This invention relates to the field of trajectory tracking technology, and more specifically, to a reversing tracking control method and system based on trajectory tracking. Background Technology

[0002] When driving a vehicle into a narrow section of road where there is no way to move forward or turn around, it is quite difficult for the average driver to reverse back the way they came. To improve driving safety in this scenario, some vehicles are now equipped with a reverse tracking function to assist drivers in reversing back the way they came within a certain distance, thereby avoiding traffic accidents.

[0003] Existing reversing tracking control methods store lateral and longitudinal parameters from forward driving, such as forward turning angle arrays, forward speed arrays, and forward driving distance, to form a corresponding fitting function, which is then read in reverse to control the vehicle's reversing. While this method can achieve reversing tracking, when the forward turning angle or speed array storage area is full, the value to be stored starts from the first element of the array and sequentially overwrites existing values. This means that the method always stores the turning angle and speed values ​​of the most recently traveled distance, making it impossible to determine the sequential relationship between adjacent storage points within the array. Consequently, subsequent data processing cannot guarantee data continuity. Furthermore, this method does not refer to the vehicle's forward driving trajectory, only recording the correspondence between the drive motor's driving distance and the electric power steering system's turning angle and speed. During reversing tracking, it cannot achieve closed-loop tracking of the trajectory, posing a risk of deviation from the forward driving trajectory and potentially leading to traffic accidents. Summary of the Invention

[0004] This invention provides a reversing tracking control method and system based on trajectory tracking, to overcome at least one technical problem existing in the prior art.

[0005] In a first aspect, embodiments of the present invention provide a reversing tracking control method based on trajectory tracking, comprising: a forward trajectory storage stage and a reversing tracking stage; wherein:

[0006] The forward trajectory storage stage includes:

[0007] Real-time acquisition of vehicle attitude coordinates and mileage; the vehicle attitude coordinates include vehicle position coordinates;

[0008] Determine whether the mileage is greater than or equal to a preset storage interval. If yes, clear the mileage to zero and store the vehicle position coordinates. If no, continue to acquire the vehicle attitude coordinates and mileage in real time.

[0009] The reversing tracking phase includes:

[0010] The vehicle position coordinates stored in the forward trajectory storage stage are read sequentially, and the total tracking mileage is calculated based on the stored vehicle position coordinates and the current vehicle position coordinates.

[0011] Determine whether the total tracking distance is less than the preset tracking distance. If yes, use the total tracking distance as the final tracking distance; otherwise, use the preset tracking distance as the final tracking distance.

[0012] The vehicle attitude coordinates and mileage are acquired in real time. The remaining mileage is calculated based on the mileage and the final tracking mileage, and the remaining mileage is tracked.

[0013] Obtain the initial target trajectory point, and calculate the distance between the current vehicle position coordinates and the initial target trajectory point;

[0014] Determine whether the distance between the current vehicle position coordinates and the initial target trajectory point is greater than the pre-aiming distance. If so, use the initial target trajectory point as the final target trajectory point; otherwise, update the initial target trajectory point and determine whether the distance between the current vehicle position coordinates and the updated initial target trajectory point is greater than the pre-aiming distance.

[0015] Calculate the steering wheel angle control value based on the final target trajectory point;

[0016] Determine if the remaining driving mileage is less than the preset distance. If yes, end the reversing tracking. If no, acquire the vehicle's attitude coordinates and driving mileage in real time, calculate the remaining driving mileage based on the driving mileage and the final tracking mileage, and track the remaining driving mileage.

[0017] Optionally, prior to the forward trajectory storage stage, the following steps are also included:

[0018] The system detects the vehicle's current gear and forward speed. If the current gear is forward and the forward speed is less than a preset speed, the system enters the forward trajectory storage stage; otherwise, the stored vehicle position coordinates are cleared.

[0019] Optionally, before storing the vehicle location coordinates, it is determined whether the storage space is full. If it is full, according to the first-in-first-out strategy, the vehicle location coordinates stored in the first address are removed, and the vehicle location coordinates to be stored are stored in the last address; otherwise, the storage address is incremented by 1, and the vehicle location coordinates to be stored are stored.

[0020] Optionally, the step of calculating the total tracking distance based on the stored vehicle position coordinates and the current vehicle position coordinates specifically involves:

[0021] Using the distance formula between two points, the stored vehicle position coordinates are accumulated and then added to the distance between the current vehicle position coordinates and the vehicle position coordinates in the last address. The total tracking distance is then calculated. Where Store_x represents the x-coordinate of the stored trajectory, Store_y represents the y-coordinate of the stored trajectory, n represents the number of sampling points for the stored trajectory coordinates, and x and y represent the vehicle position coordinates.

[0022] Optionally, obtaining the initial target trajectory point and calculating the distance between the current vehicle position coordinates and the initial target trajectory point specifically involves:

[0023] Read the vehicle position coordinates stored in the last address as the initial target trajectory point;

[0024] Get the current vehicle location coordinates;

[0025] Calculate the distance between the current vehicle position coordinates and the initial target trajectory point. Where x_Ta and y_Ta represent the coordinates of the initial target trajectory point, and x and y represent the coordinates of the current vehicle position.

[0026] Optionally, updating the initial target trajectory point specifically involves:

[0027] Determine whether the current address is the first address of the storage space. If so, use the vehicle position coordinates stored in the current address as the initial target trajectory point; otherwise, read the vehicle position coordinates stored in the previous address and use the vehicle position coordinates stored in the previous address as the initial target trajectory point.

[0028] Optionally, in the vehicle coordinate system, the coordinates of the rear axle center are the vehicle position coordinates; the calculation of the steering wheel angle control value based on the final target trajectory point specifically involves:

[0029] If the rear axle center of the vehicle passes through the final target trajectory point, then the turning radius of the rear axle center of the vehicle is... Where x_Tb and y_Tb represent the coordinates of the final target trajectory point, θ represents the vehicle heading angle, and x and y represent the vehicle position coordinates;

[0030] Let the vehicle wheelbase be L, and the transmission ratio from steering wheel angle to wheel angle be μ, then the steering wheel angle control value is...

[0031] Secondly, embodiments of the present invention provide a reversing tracking control system based on trajectory tracking, comprising:

[0032] The data acquisition module is used to acquire vehicle attitude coordinates and mileage in real time; the vehicle attitude coordinates include vehicle position coordinates.

[0033] The judgment module is used to determine whether the driving mileage is greater than or equal to a preset storage interval. If yes, the driving mileage is cleared to zero and the vehicle position coordinates are stored; otherwise, the vehicle attitude coordinates and driving mileage are acquired in real time.

[0034] The calculation module is used to sequentially read the vehicle position coordinates stored in the forward trajectory storage stage, and calculate the total tracking mileage based on the stored vehicle position coordinates and the current vehicle position coordinates.

[0035] The judgment module is also used to determine whether the total tracking mileage is less than the preset tracking mileage. If so, the total tracking mileage is used as the final tracking mileage; otherwise, the preset tracking mileage is used as the final tracking mileage.

[0036] The calculation module is also used to calculate the remaining driving mileage based on the driving mileage and the final tracking mileage;

[0037] The trajectory point acquisition module is used to acquire an initial target trajectory point and calculate the distance between the current vehicle position coordinates and the initial target trajectory point;

[0038] The judgment module is also used to determine whether the distance between the current vehicle position coordinates and the initial target trajectory point is greater than the pre-aiming distance. If so, the initial target trajectory point is used as the final target trajectory point; otherwise, the initial target trajectory point is updated, and it is determined whether the distance between the current vehicle position coordinates and the updated initial target trajectory point is greater than the pre-aiming distance.

[0039] An angle control value determination module is used to calculate the steering wheel angle control value based on the final target trajectory point;

[0040] The judgment module is also used to determine whether the remaining driving mileage is less than a preset distance. If so, the reversing tracking ends; if not, the vehicle posture coordinates and driving mileage are acquired in real time, the remaining driving mileage is calculated based on the driving mileage and the final tracking mileage, and the remaining driving mileage is tracked.

[0041] Optionally, it also includes:

[0042] The detection module is used to detect the vehicle's current gear and forward driving speed. If the current gear is forward and the forward driving speed is less than the preset speed, the vehicle enters the forward trajectory storage stage; otherwise, the stored vehicle position coordinates are cleared.

[0043] Optionally, the judgment module is further configured to determine whether the storage space is full. If it is full, the first stored vehicle position coordinates are removed according to the first-in-first-out strategy, and the vehicle position coordinates to be stored are stored in the last address. Otherwise, the storage address is incremented by 1, and the vehicle position coordinates to be stored are stored.

[0044] The innovative aspects of this invention include:

[0045] 1. In this embodiment, closed-loop trajectory tracking during reversing can be achieved by storing the vehicle's attitude coordinates while it is moving forward. This avoids the problem of deviation between the reversing and forward-moving trajectories, thereby effectively reducing the risk of traffic accidents. This is one of the innovative points of this embodiment.

[0046] 2. In this embodiment, closed-loop trajectory tracking during reversing can be achieved by storing the vehicle's attitude coordinates while it is moving forward. Compared with the method that requires storing the steering wheel angle, speed, and driving distance at the same time, this method saves storage space and reduces the complexity of the reversing trajectory process. This is one of the innovative points of this embodiment.

[0047] 3. In this embodiment, by setting a pre-aiming distance, updating the target trajectory point, and deriving and calculating the real-time steering wheel angle, the vehicle's lateral control is achieved, thereby realizing trajectory tracking when reversing. This is one of the innovative points of this embodiment. Attached Figure Description

[0048] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0049] Figure 1 A flowchart of a reversing tracking control method provided in an embodiment of the present invention;

[0050] Figure 2 A flowchart of the forward trajectory storage stage provided in an embodiment of the present invention;

[0051] Figure 3 A flowchart of the reversing tracking stage provided in an embodiment of the present invention;

[0052] Figure 4 Another flowchart of the reversing tracking control method provided in the embodiment of the present invention;

[0053] Figure 5 Another flowchart for the forward trajectory storage stage provided in an embodiment of the present invention;

[0054] Figure 6 A flowchart for calculating Sm provided in an embodiment of the present invention;

[0055] Figure 7 A flowchart for updating the initial target trajectory points provided in an embodiment of the present invention;

[0056] Figure 8 A schematic diagram of a reversing tracking control system provided in an embodiment of the present invention;

[0057] Figure 9 This is another schematic diagram of the reversing tracking control system provided in an embodiment of the present invention. Detailed Implementation

[0058] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0059] It should be noted that the terms "comprising" and "having," and any variations thereof, in the embodiments and drawings of this invention are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or device that includes a series of steps or units is not limited to the steps or units listed, but may optionally include steps or units not listed, or may optionally include other steps or units inherent to these processes, methods, products, or devices.

[0060] This invention discloses a reversing tracking control method and system based on trajectory tracking. These will be described in detail below.

[0061] Figure 1 This is a flowchart of a reversing tracking control method provided in an embodiment of the present invention. Figure 2 This is a flowchart of the forward trajectory storage stage provided in an embodiment of the present invention. Figure 3 Please refer to the flowchart of the reversing tracking stage provided in the embodiment of the present invention. Figures 1-3The reversing trajectory control method based on trajectory tracking provided in this application includes a forward trajectory storage stage (Step 1) and a reversing trajectory tracking stage (Step 2). The reversing trajectory tracking control method is applied to a reversing trajectory tracking control system, which is equipped with a function switch. This function switch can be a physical button on the reversing trajectory tracking control system or a virtual button integrated into the central control screen. When the function switch is turned on, the system enters the reversing trajectory tracking stage (Step 2). When the function switch is not turned on, the system determines whether to enter the forward trajectory storage stage (Step 1) by detecting the vehicle's gear and forward speed. For example, when the vehicle's current gear is forward and the vehicle's forward speed is less than a preset speed, recording the vehicle trajectory begins, i.e., entering the forward trajectory storage stage (Step 1).

[0062] Step 1, the forward trajectory storage stage, includes:

[0063] Step 11: Obtain vehicle attitude coordinates and mileage in real time; vehicle attitude coordinates include vehicle position coordinates;

[0064] Step 12: Determine if the mileage is greater than or equal to the preset storage interval. If yes, clear the mileage to zero and store the vehicle position coordinates. If no, continue to acquire the vehicle attitude coordinates and mileage in real time.

[0065] Step 2, the reversing tracking stage, includes:

[0066] Step 21: Read the vehicle position coordinates stored in the forward trajectory storage stage in sequence, and calculate the total tracking mileage based on the stored vehicle position coordinates and the current vehicle position coordinates;

[0067] Step 22: Determine whether the total tracking distance is less than the preset tracking distance. If yes, use the total tracking distance as the final tracking distance; otherwise, use the preset tracking distance as the final tracking distance.

[0068] Step 23: Obtain vehicle attitude coordinates and mileage in real time, calculate the remaining mileage based on the mileage and the final tracking mileage, and track the remaining mileage.

[0069] Step 24: Obtain the initial target trajectory point and calculate the distance between the current vehicle position coordinates and the initial target trajectory point;

[0070] Step 25: Determine whether the distance between the current vehicle position coordinates and the initial target trajectory point is greater than the pre-aiming distance. If so, use the initial target trajectory point as the final target trajectory point; otherwise, update the initial target trajectory point and determine whether the distance between the current vehicle position coordinates and the updated initial target trajectory point is greater than the pre-aiming distance.

[0071] Step 26: Calculate the steering wheel angle control value based on the final target trajectory point;

[0072] Step 27: Determine if the remaining driving mileage is less than the preset distance. If yes, end the reversing tracking. If no, obtain the vehicle's attitude coordinates and driving mileage in real time, calculate the remaining driving mileage based on the driving mileage and the final tracking mileage, and track the remaining driving mileage.

[0073] For details, please refer to Figures 1-3 When the function switch is not activated and the vehicle status meets the requirements, the tracking system begins to save the vehicle trajectory. It should be noted that when saving the vehicle trajectory, only the most recently traveled path needs to be saved. For example, if the preset tracking distance is set to Sd, then the location information within the most recently traveled distance Sd needs to be saved. Vehicle location information is saved at equal intervals, for example, every preset storage interval Sr. To ensure that each acquired vehicle location information can be saved to the storage space, the storage space must be greater than Sd / Sr, ensuring that the storage space can hold the vehicle trajectory within the preset tracking distance Sd.

[0074] Please refer to Figure 2 During the forward trajectory storage phase, the vehicle attitude coordinates are first acquired in real time via step 11. These coordinates include the vehicle's current position coordinates (x, y) and heading angle θ, both referenced to the vehicle coordinate system. Therefore, at vehicle startup, a vehicle coordinate system is first established, with the rear axle center as the origin, the forward direction as the x-axis, and the left side as the y-axis. Since the rear axle center is used as the reference point, the vehicle attitude coordinates and position coordinates in this application refer to the coordinates of the rear axle center.

[0075] In step 11, in addition to acquiring the vehicle attitude coordinates, it is also necessary to acquire the vehicle mileage in real time. Before the initial calculation, the mileage Sa is initialized to 0. The distance change ΔS between two adjacent samples is recorded using wheel speed pulse information, and the vehicle mileage Sa = Sa + ΔS is calculated. Then, in step 12, the relationship between the mileage Sa and the preset storage interval Sr is determined. When the mileage Sa is greater than or equal to the preset storage interval Sr, it indicates that the location to be stored has been reached. The vehicle position coordinates are stored in the storage space, and the mileage Sa is cleared to zero for the next mileage calculation. When the mileage Sa is less than the preset storage interval Sr, the vehicle attitude coordinates and mileage are acquired in real time again, and steps 11 and 12 are repeated.

[0076] After the forward trajectory is stored in the storage space, the function switch is turned on to enter the reversing tracking stage. The tracking system is engaged in reverse gear. In step 21, the vehicle position coordinates stored in the forward trajectory storage stage are read sequentially, and the total tracking distance Sg is calculated. In step 22, the total tracking distance Sg and the preset tracking distance Sd are compared, and the smaller value is taken as the final tracking distance Sz. After obtaining the final tracking distance Sz, the vehicle attitude coordinates and mileage are acquired in real time in step 23. The mileage calculation method here can refer to step 11. The remaining mileage Se is calculated based on the mileage Sa and the final tracking distance Sz, where Se = Sz - Sa, thus obtaining the remaining tracking mileage Se.

[0077] When tracking the remaining driving distance Se, firstly, the initial target trajectory point (x_Ta, y_Ta) is obtained through step 24, and the current vehicle position coordinates (x, y) are obtained. The distance Sm between the current vehicle position coordinates (x, y) and the initial target trajectory point (x_Ta, y_Ta) is calculated. In step 25, a pre-aiming distance Sp is set, and the size of Sm and the pre-aiming distance Sp is compared. When Sm is greater than Sp, the initial target trajectory point is used as the final target trajectory point. When Sm is less than or equal to Sp, the initial target trajectory point (x_Ta, y_Ta) is updated. Then, steps 24 and 25 are repeated for the updated initial target trajectory point (x_Ta, y_Ta) until the final target trajectory point is obtained. It should be noted that in the reversing tracking stage, when the initial target trajectory point is obtained for the first time, the last vehicle position coordinates stored in the forward storage stage are used as the initial target trajectory point.

[0078] In the reversing tracking stage of this invention, after the function switch is turned on, the vehicle's longitudinal control drives the vehicle to move at a constant speed, and then drives the vehicle to stop at the end. Therefore, reversing tracking mainly refers to controlling the steering wheel angle. Thus, after obtaining the final target trajectory point, the steering wheel angle control value is calculated in step 26. Through this angle control value, the steering wheel angle can be determined, thereby realizing closed-loop lateral control of the vehicle.

[0079] During the reversing tracking phase, it is necessary to continuously determine whether the reversing tracking has ended. For example, in step 27, the reversing tracking is determined by comparing the remaining driving mileage with the preset distance. When the remaining driving mileage is less than the preset distance, the reversing tracking ends; when the remaining driving mileage is greater than or equal to the preset distance, steps 23 to 27 are repeated until the remaining driving mileage is less than the preset distance.

[0080] The reversing trajectory control method provided by this invention achieves closed-loop trajectory tracking during reversing by storing the vehicle's attitude coordinates while it is traveling forward. This method is applicable to various types of intelligent vehicles, effectively reducing the risk of traffic accidents by avoiding deviations between the reversing and forward trajectories. It also saves storage space and reduces the complexity of the reversing trajectory process. Furthermore, this invention achieves lateral control of the vehicle by setting a pre-aiming distance, updating the target trajectory points, and deriving and calculating the real-time steering wheel angle, thereby enabling trajectory tracking during reversing.

[0081] Optionally, Figure 4 For another flowchart of the reversing tracking control method provided in this embodiment of the invention, please refer to... Figure 4 Before the forward trajectory storage stage, step 01 is also included: detecting the vehicle's current gear and forward driving speed. If the current gear is forward and the forward driving speed is less than the preset speed, then the forward trajectory storage stage is entered; otherwise, the stored vehicle position coordinates are cleared.

[0082] For details, please refer to Figure 4 When the function switch is not activated, the current vehicle status needs to be detected to determine whether the forward trajectory needs to be stored. Measuring the current status includes, for example, the vehicle's gear position and forward speed. When the vehicle's current gear is forward and its forward speed is less than a preset speed, it indicates that the vehicle trajectory needs to be saved, and the reversing tracking system begins recording and storing the vehicle's position information within the most recently traveled preset tracking distance Sd. When the vehicle's current gear is not forward, or its forward speed is greater than or equal to the preset speed, the vehicle's position coordinates already stored in the storage space are cleared. This invention, by detecting the vehicle's current status to determine whether information needs to be saved, avoids storing invalid information, simplifying the operation process; simultaneously, clearing invalid information from the storage space saves storage space.

[0083] Optionally, Figure 5 For another flowchart of the forward trajectory storage stage provided in this embodiment of the invention, please refer to... Figure 5 In step 12, before storing the vehicle location coordinates, it is first determined whether the storage space is full. If it is full, according to the first-in-first-out strategy, the vehicle location coordinates stored in the first address are removed, and the vehicle location coordinates to be stored are stored in the last address; otherwise, the storage address is incremented by 1, and the vehicle location coordinates to be stored are stored.

[0084] For details, please refer to Figure 5In step 12, before storing the vehicle position coordinates, it is first determined whether the storage space is full. If the storage space is full, a first-in-first-out (FIFO) strategy is executed, shifting the data addresses in the storage space sequentially to the right. The vehicle position coordinates stored in the first address (i.e., the earliest stored vehicle position coordinates) are removed, and the vehicle position coordinates to be stored are stored in the last address. If the storage space is not full, the storage address is incremented by 1, and the vehicle position coordinates to be stored are stored in that address. In the forward trajectory storage stage, this invention stores vehicle position information at equal intervals. When the storage area is full, the most recently traveled distance is saved using the FIFO strategy, ensuring that the vehicle position information stored in the storage space is a continuous set of points, avoiding inaccurate trajectory tracking due to discontinuous data.

[0085] Alternatively, please refer to Figure 3 In step 21, the total tracking mileage is calculated based on the stored vehicle position coordinates and the current vehicle position coordinates. Specifically, the stored vehicle position coordinates are summed using the distance formula between two points, and then the distance between the current vehicle position coordinates and the vehicle position coordinates in the last address is added to obtain the total tracking mileage. Where Store_x represents the x-coordinate of the storage space, Store_y represents the y-coordinate of the storage space, n represents the storage space address, and x and y represent the vehicle position coordinates.

[0086] For details, please refer to Figure 3 In step 21, when calculating the total tracking distance, the distance between adjacent points stored in the storage space is calculated using the distance formula between two points, and the distances between all adjacent points are accumulated. Then, obtain the current vehicle position coordinates and calculate the distance between the current vehicle position coordinates and the vehicle position coordinates stored in the last address. Adding the two together gives the total tracking distance. Where Store_x represents the x-coordinate of the stored trajectory, Store_y represents the y-coordinate of the stored trajectory, n represents the number of sampling points for the stored trajectory coordinates, and x and y represent the vehicle position coordinates.

[0087] Then, the total tracking distance Sg is compared with the preset tracking distance Sd. If the total tracking distance Sg is less than the preset tracking distance Sd, the total tracking distance Sg is taken as the final tracking distance Sz; if the total tracking distance Sg is less than the preset tracking distance Sd, the preset tracking distance Sd is taken as the final tracking distance Sz. This invention obtains the total tracking distance Sg by reading and accumulating the stored vehicle position coordinates, and then compares the total tracking distance Sg with the set preset tracking distance Sd to obtain the final total reversing tracking distance.

[0088] Optionally, Figure 6 For a flowchart of calculating Sm provided in an embodiment of the present invention, please refer to [the flowchart]. Figure 6 In step 24, the initial target trajectory point is obtained, and the distance between the current vehicle position coordinates and the initial target trajectory point is calculated. Specifically: Step 241: Read the vehicle position coordinates stored in the last address as the initial target trajectory point; Step 242: Obtain the current vehicle position coordinates; Step 243: Calculate the distance between the current vehicle position coordinates and the initial target trajectory point. Where x_Ta and y_Ta represent the coordinates of the initial target trajectory point, and x and y represent the coordinates of the current vehicle position.

[0089] For details, please refer to Figure 6 In the reversing tracking phase, when the initial target trajectory point is first acquired, the last vehicle position coordinate stored in the forward storage phase is used as the initial target trajectory point. The last vehicle position coordinate stored in the forward storage phase is the vehicle position coordinate stored in the last address of the storage space. In step 24, the vehicle position coordinate stored in the last address is first read in step 241 as the initial target trajectory point. In step 242, the current vehicle position coordinate is acquired. After obtaining the initial target trajectory point and the current vehicle position coordinate, the distance between the current vehicle position coordinate and the initial target trajectory point is calculated in step 243. Where x_Ta and y_Ta represent the coordinates of the initial target trajectory point, and x and y represent the coordinates of the current vehicle position. This invention continuously updates the initial target trajectory point by setting a pre-aiming distance and comparing the magnitudes of Sm and the pre-aiming distance Sp, deriving and calculating the real-time steering wheel angle to perform lateral control of the vehicle, thereby achieving trajectory tracking when reversing.

[0090] Optionally, Figure 7 For a flowchart of updating the initial target trajectory points provided in an embodiment of the present invention, please refer to... Figure 7 In step 25, the initial target trajectory point is updated. Specifically, it is determined whether the current address is the first address of the storage space. If so, the vehicle position coordinates stored in the current address are used as the initial target trajectory point; otherwise, the vehicle position coordinates stored in the previous address are read and used as the initial target trajectory point.

[0091] For details, please refer to Figure 7When updating the initial target trajectory point, the vehicle position coordinates stored in the previous address are taken as the initial target trajectory point. However, when reaching the first address in the storage space, the previous address does not exist, and the program encounters an error. Therefore, in this embodiment, it is first determined whether the current address is the first address in the storage space. If so, the vehicle position coordinates stored in the current address are used as the initial target trajectory point; otherwise, the vehicle position coordinates stored in the previous address are read and used as the initial target trajectory point. This invention continuously updates the initial target trajectory point to derive and calculate the real-time steering wheel angle for lateral vehicle control, thereby achieving trajectory tracking during reversing.

[0092] Optionally, in the vehicle coordinate system, the coordinates of the rear axle center are the vehicle position coordinates; please refer to... Figure 3 In step 26, the steering wheel angle control value is calculated based on the final target trajectory point. Specifically, if the rear axle center of the vehicle passes through the final target trajectory point, then the turning radius of the rear axle center is... Where x_Tb and y_Tb represent the coordinates of the final target trajectory point, θ represents the vehicle heading angle, and x and y represent the current vehicle position coordinates; assuming the vehicle wheelbase is L and the transmission ratio from steering wheel angle to wheel angle is μ, then the steering wheel angle control value is...

[0093] Specifically, assuming the rear axle center of the vehicle passes through the final target trajectory point (x_Tb, y_Tb), the current rear axle center coordinates are (x, y), the circle center coordinates are (x0, y0), and the turning radius of the rear axle center is R, then we can obtain x0 = xR*sin(θ). R 2 =(x_Tb-x0) 2 +(y_Tb-y0) 2 The derivation yields... Thus obtain Then, assuming the vehicle wheelbase is L and the transmission ratio from steering wheel angle to wheel angle is μ, the steering wheel angle control value is... This invention stores the vehicle's attitude coordinates while it is moving forward and updates the target trajectory points. This allows for the derivation and calculation of real-time steering wheel angle control values, enabling lateral control of the vehicle and trajectory tracking during reversing.

[0094] Based on the same inventive concept, the present invention also provides a reversing tracking control system based on trajectory tracking. Figure 8 This is a schematic diagram of a reversing tracking control system provided in an embodiment of the present invention. Please refer to... Figure 8 The reversing tracking control system 100 includes:

[0095] Data acquisition module 10 is used to acquire vehicle attitude coordinates and mileage in real time; vehicle attitude coordinates include vehicle position coordinates.

[0096] The judgment module 20 is used to determine whether the driving mileage is greater than or equal to the preset storage interval. If yes, the driving mileage is cleared to zero and the vehicle position coordinates are stored; otherwise, the vehicle attitude coordinates and driving mileage are acquired in real time.

[0097] The calculation module 30 is used to sequentially read the vehicle position coordinates stored in the forward trajectory storage stage, and calculate the total tracking mileage based on the stored vehicle position coordinates and the current vehicle position coordinates.

[0098] The judgment module 20 is also used to determine whether the total tracking distance is less than the preset tracking distance. If so, the total tracking distance is used as the final tracking distance; otherwise, the preset tracking distance is used as the final tracking distance.

[0099] The calculation module 30 is also used to calculate the remaining driving range based on the driving range and the final tracking range;

[0100] The trajectory point acquisition module 40 is used to acquire the initial target trajectory point and calculate the distance between the current vehicle position coordinates and the initial target trajectory point;

[0101] The judgment module 20 is also used to determine whether the distance between the current vehicle position coordinates and the initial target trajectory point is greater than the pre-aiming distance. If so, the initial target trajectory point is used as the final target trajectory point; otherwise, the initial target trajectory point is updated, and it is determined whether the distance between the current vehicle position coordinates and the updated initial target trajectory point is greater than the pre-aiming distance.

[0102] The steering angle control value determination module 50 is used to calculate the steering wheel angle control value based on the final target trajectory point;

[0103] The judgment module 20 is also used to determine whether the remaining driving mileage is less than the preset distance. If so, the reversing tracking ends; if not, the vehicle attitude coordinates and driving mileage are obtained in real time, the remaining driving mileage is calculated based on the driving mileage and the final tracking mileage, and the remaining driving mileage is tracked.

[0104] For details, please refer to Figure 8The reversing tracking control system 100 based on trajectory tracking provided by this invention includes a data acquisition module 10 and a judgment module 20. During the forward trajectory storage phase, the data acquisition module 10 acquires the vehicle's attitude coordinates in real time. The vehicle attitude coordinates include the vehicle's current position coordinates (x, y) and heading angle θ, both of which are referenced to the vehicle coordinate system. Therefore, at the moment of vehicle startup, a vehicle coordinate system is first established. The vehicle coordinate system has the rear axle center position as the origin, the vehicle's forward direction as the x-axis, and the left side of the vehicle as the y-axis. Since the vehicle coordinate system uses the rear axle center position as the reference point, the vehicle attitude coordinates and vehicle position coordinates in this application both refer to the coordinates of the rear axle center.

[0105] In addition to acquiring vehicle attitude coordinates, the data acquisition module 10 also needs to acquire vehicle mileage in real time. Before the initial calculation, the mileage Sa is initialized to 0. The distance change ΔS between two adjacent samples is recorded using wheel speed pulse information, and the vehicle mileage Sa = Sa + ΔS is calculated. Then, the judgment module 20 determines the relationship between the mileage Sa and the preset storage interval Sr. When the mileage Sa is greater than or equal to the preset storage interval Sr, it indicates that the location to be stored has been reached. The current vehicle position coordinates are stored in the storage space, and the mileage Sa is cleared to zero for the next calculation. When the mileage Sa is less than the preset storage interval Sr, the above process is repeated using the data acquisition module 10 and the judgment module 20.

[0106] The reversing tracking control system 100 also includes a function switch 70 and a calculation module 30. After the forward trajectory is stored in the storage space, the function switch 70 is turned on to enter the reversing tracking stage, and the tracking system engages reverse gear. The calculation module 30 sequentially reads the vehicle position coordinates stored in the forward trajectory storage stage and calculates the total tracking distance Sg. The judgment module 20 judges the magnitude of the total tracking distance Sg and the preset tracking distance Sd, and takes the smaller value as the final tracking distance Sz. After obtaining the final tracking distance Sz, the data acquisition module 10 again uses real-time acquisition of vehicle attitude coordinates and mileage, and the calculation module 30 uses the mileage Sa and the final tracking distance Sz to calculate the remaining mileage Se, Se = Sz - Sa, thus obtaining the remaining tracking mileage Se.

[0107] When the remaining driving distance Se is reached during the reversing tracking phase, the initial target trajectory point (x_Ta, y_Ta) is obtained through the trajectory point acquisition module 40, and the current vehicle position coordinates (x, y) are also obtained. The distance Sm between the current vehicle position coordinates (x, y) and the initial target trajectory point (x_Ta, y_Ta) is calculated. Then, the judgment module 20 compares Sm with the pre-aiming distance Sp. When Sm is greater than Sp, the initial target trajectory point is used as the final target trajectory point; when Sm is less than or equal to Sp, the initial target trajectory point (x_Ta, y_Ta) is updated. It should be noted that during the reversing tracking phase, when the initial target trajectory point is obtained for the first time, the last vehicle position coordinates stored in the forward storage phase are used as the initial target trajectory point.

[0108] After obtaining the final target trajectory point, the steering wheel angle control value is calculated using the steering angle control value determination module 50. The steering wheel angle can be determined using this steering angle control value, thereby realizing closed-loop lateral control of the vehicle.

[0109] During the reversing tracking phase, it is necessary to constantly determine whether the reversing tracking has ended. Therefore, the judgment module 20 compares the remaining driving mileage with the preset distance to determine whether the reversing tracking has ended. When the remaining driving mileage is less than the preset distance, the reversing tracking ends; when the remaining driving mileage is greater than or equal to the preset distance, the above process is repeated until the remaining driving mileage is less than the preset distance.

[0110] The reversing trajectory tracking control system 100 provided by this invention can achieve closed-loop trajectory tracking during reversing by storing the vehicle's attitude coordinates while it is traveling forward. This not only avoids deviations between the reversing and forward trajectories, effectively reducing the risk of traffic accidents, but also saves storage space and reduces the complexity of the reversing trajectory tracking process. Furthermore, this invention achieves lateral control of the vehicle by setting a pre-aiming distance, updating the target trajectory points, and deriving and calculating the real-time steering wheel angle, thereby enabling trajectory tracking during reversing.

[0111] Optionally, Figure 9 For another structural schematic diagram of the reversing tracking control system 100 provided in this embodiment of the invention, please refer to... Figure 9 The reversing tracking control system 100 based on trajectory tracking provided in this embodiment also includes: a detection module 60, used to detect the current gear and forward driving speed of the vehicle. If the current gear is forward and the forward driving speed is less than the preset speed, the forward trajectory storage stage is entered; otherwise, the stored vehicle position coordinates are cleared.

[0112] For details, please refer to Figure 9When the function switch 70 is not activated, the current vehicle status needs to be detected to determine whether the forward trajectory needs to be stored. The reversing tracking control system 100 provided in this embodiment also includes a detection module 60, which measures the current vehicle status, such as the vehicle gear and forward speed. When the vehicle's current gear is forward and the forward speed is less than a preset speed, it indicates that the vehicle trajectory needs to be saved, and the reversing tracking system begins to record and store the position information within the preset tracking mileage Sd most recently traveled by the vehicle. When the vehicle's current gear is not forward, or the vehicle's forward speed is greater than or equal to the preset speed, the vehicle position coordinates already stored in the storage space are cleared. This invention, by detecting the current vehicle status to determine whether information needs to be saved, avoids storing invalid information, which simplifies the operation process; at the same time, clearing invalid information in the storage space saves storage space.

[0113] Alternatively, please refer to Figure 8 The judgment module 20 is also used to determine whether the storage space is full. If it is full, the first stored vehicle position coordinate is removed according to the first-in-first-out strategy, and the vehicle position coordinate to be stored is stored in the last address; otherwise, the storage address is incremented by 1, and the vehicle position coordinate to be stored is stored.

[0114] For details, please refer to Figure 8 When storing vehicle position coordinates, the first step is to determine whether the storage space is full using the judgment module 20. If the storage space is full, a first-in-first-out (FIFO) strategy is executed, shifting the data addresses in the storage space sequentially to the right. The vehicle position coordinates stored in the first address (i.e., the earliest stored vehicle position coordinates) are removed, and the vehicle position coordinates to be stored are stored in the last address. If the storage space is not full, the storage address is incremented by 1, and the vehicle position coordinates to be stored are stored in that address. In the forward trajectory storage stage, this invention stores vehicle position information at equal intervals. When the storage area is full, the FIFO strategy saves the most recently traveled distance, ensuring that the vehicle position information stored in the storage space is a continuous set of points, thus avoiding inaccurate trajectory tracking due to discontinuous data.

[0115] Those skilled in the art will understand that the accompanying drawings are merely schematic diagrams of one embodiment, and the modules or processes shown in the drawings are not necessarily essential for implementing the present invention.

[0116] Those skilled in the art will understand that the modules in the apparatus of the embodiments can be distributed in the apparatus of the embodiments as described in the embodiments, or they can be located in one or more devices different from this embodiment with corresponding changes. The modules of the above embodiments can be combined into one module, or they can be further divided into multiple sub-modules.

[0117] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A reversing tracking control method based on trajectory tracking, characterized in that, include: The forward trajectory storage stage and the reverse tracking stage; among which: The forward trajectory storage stage includes: Real-time acquisition of vehicle attitude coordinates and mileage; the vehicle attitude coordinates include vehicle position coordinates; Determine whether the mileage is greater than or equal to a preset storage interval. If yes, clear the mileage to zero and store the vehicle position coordinates. If no, continue to acquire the vehicle attitude coordinates and mileage in real time. The reversing tracking phase includes: The vehicle position coordinates stored in the forward trajectory storage stage are read sequentially, and the total tracking mileage is calculated based on the stored vehicle position coordinates and the current vehicle position coordinates. Determine whether the total tracking distance is less than the preset tracking distance. If yes, use the total tracking distance as the final tracking distance; otherwise, use the preset tracking distance as the final tracking distance. The vehicle attitude coordinates and mileage are acquired in real time. The remaining mileage is calculated based on the mileage and the final tracking mileage, and the remaining mileage is tracked. Obtain the initial target trajectory point, and calculate the distance between the current vehicle position coordinates and the initial target trajectory point; Determine whether the distance between the current vehicle position coordinates and the initial target trajectory point is greater than the pre-aiming distance. If so, use the initial target trajectory point as the final target trajectory point; otherwise, update the initial target trajectory point and determine whether the distance between the current vehicle position coordinates and the updated initial target trajectory point is greater than the pre-aiming distance. Calculate the steering wheel angle control value based on the final target trajectory point; Determine if the remaining driving mileage is less than the preset distance. If yes, end the reversing tracking. If no, acquire the vehicle's attitude coordinates and driving mileage in real time, calculate the remaining driving mileage based on the driving mileage and the final tracking mileage, and track the remaining driving mileage.

2. The reversing tracking control method based on trajectory tracking according to claim 1, characterized in that, Prior to the forward trajectory storage stage, the following is also included: The system detects the vehicle's current gear and forward speed. If the current gear is forward and the forward speed is less than a preset speed, the system enters the forward trajectory storage stage; otherwise, the stored vehicle position coordinates are cleared.

3. The reversing tracking control method based on trajectory tracking according to claim 1, characterized in that, Before storing the vehicle location coordinates, it is determined whether the storage space is full. If it is full, according to the first-in-first-out strategy, the vehicle location coordinates stored in the first address are removed, and the vehicle location coordinates to be stored are stored in the last address. Otherwise, increment the storage address by 1 and store the vehicle location coordinates to be stored.

4. The reversing tracking control method based on trajectory tracking according to claim 1, characterized in that, The step of calculating the total tracking distance based on the stored vehicle position coordinates and the current vehicle position coordinates is as follows: Using the distance formula between two points, the stored vehicle position coordinates are accumulated and then added to the distance between the current vehicle position coordinates and the vehicle position coordinates in the last address. The total tracking distance is then calculated. Where Store_x represents the x-coordinate of the stored trajectory, Store_y represents the y-coordinate of the stored trajectory, n represents the number of sampling points for the stored trajectory coordinates, and x and y represent the vehicle position coordinates.

5. The reversing tracking control method based on trajectory tracking according to claim 1, characterized in that, The process of obtaining the initial target trajectory point and calculating the distance between the current vehicle position coordinates and the initial target trajectory point specifically involves: Read the vehicle position coordinates stored in the last address as the initial target trajectory point; Get the current vehicle location coordinates; Calculate the distance between the current vehicle position coordinates and the initial target trajectory point. Where x_Ta and y_Ta represent the coordinates of the initial target trajectory point, and x and y represent the coordinates of the current vehicle position.

6. The reversing tracking control method based on trajectory tracking according to claim 5, characterized in that, The update of the initial target trajectory point specifically involves: Determine whether the current address is the first address of the storage space. If so, use the vehicle position coordinates stored in the current address as the initial target trajectory point; otherwise, read the vehicle position coordinates stored in the previous address and use the vehicle position coordinates stored in the previous address as the initial target trajectory point.

7. The reversing tracking control method based on trajectory tracking according to claim 6, characterized in that, In the vehicle coordinate system, the coordinates of the rear axle center are the vehicle position coordinates; the calculation of the steering wheel angle control value based on the final target trajectory point specifically involves: If the rear axle center of the vehicle passes through the final target trajectory point, then the turning radius of the rear axle center of the vehicle is... Where x_Tb and y_Tb represent the coordinates of the final target trajectory point, θ represents the vehicle heading angle, and x and y represent the vehicle position coordinates; Let the vehicle wheelbase be L, and the transmission ratio from steering wheel angle to wheel angle be μ, then the steering wheel angle control value is...

8. A reversing tracking control system based on trajectory tracking, characterized in that, include: The data acquisition module is used to acquire vehicle attitude coordinates and mileage in real time; the vehicle attitude coordinates include vehicle position coordinates. The judgment module is used to determine whether the driving mileage is greater than or equal to a preset storage interval. If yes, the driving mileage is cleared to zero and the vehicle position coordinates are stored; otherwise, the vehicle attitude coordinates and driving mileage are acquired in real time. The calculation module is used to sequentially read the vehicle position coordinates stored in the forward trajectory storage stage, and calculate the total tracking mileage based on the stored vehicle position coordinates and the current vehicle position coordinates. The judgment module is also used to determine whether the total tracking mileage is less than the preset tracking mileage. If so, the total tracking mileage is used as the final tracking mileage; otherwise, the preset tracking mileage is used as the final tracking mileage. The calculation module is also used to calculate the remaining driving mileage based on the driving mileage and the final tracking mileage; The trajectory point acquisition module is used to acquire an initial target trajectory point and calculate the distance between the current vehicle position coordinates and the initial target trajectory point; The judgment module is also used to determine whether the distance between the current vehicle position coordinates and the initial target trajectory point is greater than the pre-aiming distance. If so, the initial target trajectory point is used as the final target trajectory point. Otherwise, update the initial target trajectory point and determine whether the distance between the current vehicle position coordinates and the updated initial target trajectory point is greater than the pre-aiming distance; An angle control value determination module is used to calculate the steering wheel angle control value based on the final target trajectory point; The judgment module is also used to determine whether the remaining driving mileage is less than a preset distance. If so, the reversing tracking ends; if not, the vehicle posture coordinates and driving mileage are acquired in real time, the remaining driving mileage is calculated based on the driving mileage and the final tracking mileage, and the remaining driving mileage is tracked.

9. The reversing tracking control system based on trajectory tracking according to claim 8, characterized in that, Also includes: The detection module is used to detect the vehicle's current gear and forward driving speed. If the current gear is forward and the forward driving speed is less than the preset speed, the vehicle enters the forward trajectory storage stage; otherwise, the stored vehicle position coordinates are cleared.

10. The reversing tracking control system based on trajectory tracking according to claim 8, characterized in that, The judgment module is also used to determine whether the storage space is full. If it is full, the first stored vehicle position coordinates are removed according to the first-in-first-out strategy, and the vehicle position coordinates to be stored are stored in the last address. Otherwise, increment the storage address by 1 and store the vehicle location coordinates to be stored.

Images