Automatic parking trajectory tracking control method and system

Abstract

The present invention proposes an automatic parking trajectory tracking control method and system. The method comprises: receiving a parking trajectory after a target vehicle begins parking, the parking trajectory comprising multiple trajectory points, extracting trajectory point information for each trajectory point from the parking trajectory; retrieving a key parameter matrix; obtaining actual position information of the target vehicle; constructing a state matrix based on the trajectory point information corresponding to the target point and the actual heading information, and calculating the front wheel angle of the target vehicle based on the state matrix and the key parameter matrix; and if the target vehicle has not reached the end point of the trajectory, controlling the target vehicle to park based on the front wheel angle of the target vehicle. The automatic parking trajectory tracking control method proposed in the present invention avoids the computational performance requirements of the vehicle-side domain controller required by LQR iteration, enabling it to run on a low-computing-power real-time system, thereby significantly reducing computational latency and improving parking control accuracy.

Description

Technical Field

[0001] The present invention relates to the technical field of automatic parking, and in particular to an automatic parking trajectory tracking control method and system. Background Art

[0002] At present, vehicles equipped with L2 assisted driving and automatic parking functions are very popular among consumers. Vehicle drivers activate the automatic parking function through the car computer, find a valid parking space through the on-board sensors, and the on-board domain controller calculates the parking trajectory. The control module then calculates the target steering wheel angle, accelerator, brake, and gear information based on the trajectory and sends it to the vehicle actuator to control the vehicle to drive according to the target trajectory.

[0003] Currently, there are two main parking trajectory tracking solutions in mass-produced parking assistance systems. One is to set a key point method. This method requires the car to be parked at a relatively fixed distance from the parking space. A multi-segment parking method is used, and several key points are set on each parking trajectory. Each key point is set with a fixed steering wheel angle. When the vehicle reaches the key point, a fixed steering wheel angle is sent to the chassis. This method has poor robustness. Once it is forced to stop by an obstacle or an error occurs, it cannot be corrected and has poor anti-interference ability. The second method is to use a geometric method to calculate the parking trajectory and a vehicle kinematic method to calculate the steering wheel angle. This method has a certain anti-interference ability, but there are situations such as sudden changes in the steering wheel angle and steering wheel shaking during parking, which leads to a delay in the calculated steering wheel angle, and then there is a problem of large parking control errors. Summary of the Invention

[0004] Based on this, the purpose of the present invention is to propose an automatic parking trajectory tracking control method and system, so that in cases of sudden changes in the steering wheel angle, steering wheel shaking during parking, etc., the key parameter matrix can be retrieved in a timely manner, and the front wheel angle of the target vehicle can be accurately calculated, thereby saving computing resources and greatly reducing the computing delay, thereby improving the parking control accuracy.

[0005] According to an automatic parking trajectory tracking control method proposed by the present invention, the method includes:

[0006] After the target vehicle starts parking, a parking trajectory is received, the parking trajectory comprising a plurality of trajectory points, and trajectory point information of each trajectory point is extracted from the parking trajectory, the trajectory point information comprising coordinate information, vehicle body target heading information, target yaw angular velocity information, target speed information, target acceleration information, and road curvature;

[0007] Obtaining a parking trajectory type according to the parking trajectory, and retrieving a corresponding key parameter matrix from a preset database according to the parking trajectory type;

[0008] Acquiring actual position information of the target vehicle positioning, wherein the actual position information includes actual coordinate information, actual heading information, and actual yaw angular velocity information of the target vehicle;

[0009] Selecting a target point from all trajectory points, and calculating a lateral error, a lateral error rate, a heading error, and a heading error rate based on trajectory point information corresponding to the target point and the actual heading information, and constructing a state matrix based on the lateral error, the lateral error rate, the heading error, and the heading error rate, so as to calculate a front wheel turning angle of the target vehicle based on the state matrix and the key parameter matrix;

[0010] Determine whether the target vehicle has reached the end of the trajectory. If the target vehicle has not reached the end of the trajectory, control the target vehicle to park according to the front wheel angle of the target vehicle.

[0011] Furthermore, the step of constructing the preset database includes:

[0012] Controlling the target vehicle at a first preset speed to perform multiple offline simulation tests for each parking trajectory type, where the parking trajectory types include single-stage reverse parking and multi-stage parking, where the multi-stage parking includes multi-stage forward parking and multi-stage reverse parking, to obtain a lateral error weight coefficient, a lateral error rate weight coefficient, a heading error weight coefficient, a heading error rate weight coefficient, and a steering wheel angle weight coefficient corresponding to each offline simulation test;

[0013] Constructing a state weighting coefficient matrix according to the lateral error weight coefficient, the lateral error rate weight coefficient, the heading error weight coefficient, and the heading error rate weight coefficient; constructing an output weighting coefficient matrix according to the steering wheel angle weight coefficient, wherein the state weighting coefficient matrix is ​​a diagonal matrix;

[0014] Screening out a state weighting coefficient matrix and an output weighting coefficient matrix that meet the first preset condition from multiple simulation tests as the final state weighting coefficient matrix and the final output weighting coefficient matrix respectively;

[0015] Set the maximum number of iterations, set the maximum iteration error, and let P n =Q,P n-1 =Q+A T P n (R+BR -1 B T P n ) -1 A, iterate the loop until P k+1 -P k If the deviation value is less than the set maximum iteration error, the iteration stops and the output P=P k ;

[0016] The key parameter matrix is ​​calculated according to the following formula:

[0017] K=-(R+B T PB) -1 B T PA

[0018] Among them, P n represents the matrix of the first iteration, P n-1 represents the matrix of the second iteration, P k+1 represents the matrix obtained at the k+1th iteration, P k represents the matrix obtained by the kth iteration, Q represents the final state weighting coefficient matrix, R represents the final output weighting coefficient matrix, A represents the state coefficient matrix, B represents the input control quantity matrix, T represents the transpose of the matrix, and K represents the key parameter matrix;

[0019] A preset database is constructed based on the obtained key parameter matrices corresponding to the one-stage reverse parking, the multi-stage forward parking, and the multi-stage reverse parking, and the preset database is pre-stored in the target vehicle.

[0020] Furthermore, the step of selecting a target point from all trajectory points includes:

[0021] The distance between each coordinate point and the target vehicle is obtained according to the coordinate information of each trajectory point and the actual coordinate information of the target vehicle, and the coordinate point with the shortest distance from all the distances between the coordinate points and the target vehicle is selected as the target point.

[0022] Furthermore, the step of calculating the lateral error, the lateral error rate, the heading error, and the heading error rate based on the trajectory point information corresponding to the target point and the actual heading information includes:

[0023] The lateral error, lateral error rate, heading error, and heading error rate are calculated using the following formulas:

[0024]

[0025] Among them, e d Indicates lateral error, Indicates the lateral error rate, Indicates heading error, represents the heading error rate, v represents the target speed, x and y represent the horizontal and vertical coordinates in the actual coordinate information respectively, x r and y r Respectively represent the horizontal and vertical coordinates in the target point coordinate information, represents the target yaw rate, Indicates the vehicle target heading information. Indicates the actual heading information. Indicates the actual yaw rate information.

[0026] Furthermore, the step of constructing a state matrix according to the lateral error, the lateral error rate, the heading error, and the heading error rate, and calculating the front wheel turning angle of the target vehicle according to the state matrix and the key parameter matrix includes:

[0027] The state matrix is ​​constructed according to the following formula:

[0028]

[0029] x(t) represents the state matrix corresponding to the target point t;

[0030] The front wheel angle of the target vehicle is calculated according to the following formula:

[0031] u(t)=-Kx(t)

[0032] Where u(t) represents the front wheel turning angle of the target vehicle at the target point t.

[0033] Furthermore, the state coefficient matrix is ​​calculated according to the following formula:

[0034]

[0035] Among them, C f and C r They represent the front and rear wheel cornering stiffness of the target vehicle, m represents the vehicle mass, l f Indicates the length from the front end of the target vehicle to the center of mass, l r represents the length from the rear end of the target vehicle to the center of mass, v x represents the longitudinal velocity of the target vehicle, v y represents the lateral velocity of the target vehicle, I z Represents the target vehicle's moment of inertia around the z-axis.

[0036] Furthermore, the input control matrix is ​​calculated according to the following formula:

[0037]

[0038] Among them, l f represents the length from the front end of the target vehicle to the center of mass, C f represents the front wheel cornering stiffness of the target vehicle, m represents the vehicle mass of the target vehicle, I z Represents the target vehicle's moment of inertia around the z-axis.

[0039] In summary, according to the above-mentioned automatic parking trajectory tracking control method, the LQR optimization control strategy is adopted to calculate the target steering wheel angle (front wheel angle). Offline iterative calculation is used for the calculation of the key parameter matrix, and the calculation results are saved locally. The LQR iterative calculation is completed offline, avoiding the LQR iteration's requirements for the computing performance of the vehicle-side domain controller, enabling it to run in a low-computing real-time system, thereby greatly reducing the calculation delay and improving the parking control accuracy. Specifically, when the target vehicle begins to park, the parking trajectory is actively received, and the corresponding key parameter matrix is ​​quickly retrieved according to the type of parking trajectory to reduce calculations. Then, the actual position information of the target vehicle is obtained, and the nearest target point is selected to construct a state matrix based on the trajectory point information and actual heading information of the target point. The front wheel angle is then calculated based on the retrieved key parameter matrix and the state matrix. Then, it is determined whether the target vehicle has completed parking. If not, the target vehicle is controlled to park according to the front wheel angle, thereby greatly improving the control accuracy and solving the serious calculation delay problem of traditional technology.

[0040] Another aspect of the present invention further provides an automatic parking trajectory tracking control system, the system comprising:

[0041] a parking trajectory acquisition module, configured to receive a parking trajectory after the target vehicle begins parking, the parking trajectory comprising a plurality of trajectory points, and extract trajectory point information of each trajectory point from the parking trajectory, the trajectory point information including coordinate information, vehicle body target heading information, target yaw angular velocity information, target speed information, target acceleration information, and road curvature;

[0042] a key parameter matrix retrieving module, configured to obtain a parking trajectory type according to the parking trajectory, and retrieve a corresponding key parameter matrix from a preset database according to the parking trajectory type;

[0043] An actual position information acquisition module is used to acquire the actual position information of the target vehicle, wherein the actual position information includes the actual coordinate information, actual heading information and actual yaw angular velocity information of the target vehicle;

[0044] a front wheel steering angle calculation module, configured to select a target point from all trajectory points, and calculate a lateral error, a lateral error rate, a heading error, and a heading error rate based on trajectory point information corresponding to the target point and the actual heading information, and construct a state matrix based on the lateral error, the lateral error rate, the heading error, and the heading error rate, so as to calculate the front wheel steering angle of the target vehicle based on the state matrix and the key parameter matrix;

[0045] The parking control module is used to determine whether the target vehicle has reached the end of the trajectory. If the target vehicle has not reached the end of the trajectory, the target vehicle is controlled to park according to the front wheel angle of the target vehicle.

[0046] Furthermore, the system further comprises:

[0047] a simulation test module, configured to control a target vehicle at a first preset vehicle speed to perform multiple offline simulation tests for each parking trajectory type, where the parking trajectory types include single-stage reverse parking and multi-stage parking, where the multi-stage parking includes multi-stage forward parking and multi-stage reverse parking, to obtain a lateral error weight coefficient, a lateral error rate weight coefficient, a heading error weight coefficient, a heading error rate weight coefficient, and a steering wheel angle weight coefficient corresponding to each offline simulation test;

[0048] A weighted coefficient matrix construction module is used to construct a state weighted coefficient matrix based on the lateral error weight coefficient, the lateral error rate weight coefficient, the heading error weight coefficient, and the heading error rate weight coefficient, and to construct an output weighted coefficient matrix based on the steering wheel angle weight coefficient. The state weighted coefficient matrix is ​​a diagonal matrix.

[0049] Screening out a state weighting coefficient matrix and an output weighting coefficient matrix that meet the first preset condition from multiple simulation tests as the final state weighting coefficient matrix and the final output weighting coefficient matrix respectively;

[0050] The loop iteration module is used to set the maximum number of iterations and the maximum iteration error. n =Q,P n-1 =Q+A T P n (R+BR -1 B T P n ) -1 A, iterate the loop until P k+1 -P k If the deviation value is less than the set maximum iteration error, the iteration stops and the output P=P k ;

[0051] The key parameter calculation module is used to calculate the key parameter matrix according to the following formula:

[0052] K=-(R+B T PB) -1 B T PA

[0053] Among them, P n represents the matrix of the first iteration, P n-1 represents the matrix of the second iteration, P k+1 represents the matrix obtained at the k+1th iteration, Pk represents the matrix obtained by the kth iteration, Q represents the final state weighting coefficient matrix, R represents the final output weighting coefficient matrix, A represents the state coefficient matrix, B represents the input control quantity matrix, T represents the transpose of the matrix, and K represents the key parameter matrix;

[0054] The preset database construction module is used to construct a preset database based on the obtained key parameter matrices corresponding to one-stage reverse parking, multi-stage forward parking and multi-stage reverse parking, and pre-store the preset database in the target vehicle.

[0055] Furthermore, the front wheel angle calculation module further includes:

[0056] The target point selection unit is used to obtain the distance between each coordinate point and the target vehicle based on the coordinate information of each trajectory point and the actual coordinate information of the target vehicle, and to select the coordinate point with the shortest distance from all the distances between the coordinate points and the target vehicle as the target point.

[0057] Additional aspects and advantages of the present invention will be set forth in part in the following description and, in part, will be obvious from the following description, or may be learned through embodiments of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1 This is a flow chart of the automatic parking trajectory tracking control method proposed in the first embodiment of the present invention;

[0059] Figure 2 This is a schematic structural diagram of the automatic parking trajectory tracking control system proposed in the second embodiment of the present invention.

[0060] The following specific embodiments will further illustrate the present invention in conjunction with the above-mentioned drawings. DETAILED DESCRIPTION

[0061] To facilitate understanding of the present invention, the present invention will be described more fully below with reference to the accompanying drawings. The drawings illustrate several embodiments of the present invention. However, the present invention may be implemented in many different forms and is not limited to the embodiments described herein. Rather, these embodiments are provided to provide a more thorough and comprehensive understanding of the present invention.

[0062] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the art to which this invention pertains. The terms used in this specification of the present invention are for the purpose of describing specific embodiments only and are not intended to limit the present invention. The term "and / or" as used herein includes any and all combinations of one or more of the associated listed items.

[0063] See also Figure 1, which is a flow chart of the automatic parking trajectory tracking control method according to the first embodiment of the present invention, the method includes steps S01 to S05, wherein:

[0064] Step S01: After a target vehicle starts parking, a parking trajectory is received, the parking trajectory comprising a plurality of trajectory points, and trajectory point information of each trajectory point is extracted from the parking trajectory, the trajectory point information including coordinate information, vehicle body target heading information, target yaw angular velocity information, target speed information, target acceleration information, and road curvature;

[0065] Step S02: obtaining a parking trajectory type according to the parking trajectory, and retrieving a corresponding key parameter matrix from a preset database according to the parking trajectory type;

[0066] It should be noted that parking trajectory types include one-stage reverse parking and multi-stage parking. Multi-stage parking includes multi-stage forward parking and multi-stage reverse parking. By quickly retrieving the key parameter matrix directly from the preset database, calculation time is greatly saved. The process of building the preset database is as follows:

[0067] Controlling a target vehicle at a first preset speed to perform multiple offline simulation tests for each parking trajectory type, the parking trajectory types including one-stage reverse parking and multi-stage parking, wherein the multi-stage parking includes multi-stage forward parking and multi-stage reverse parking, to obtain a lateral error weight coefficient, a lateral error rate weight coefficient, a heading error weight coefficient, a heading error rate weight coefficient, and a steering wheel angle weight coefficient corresponding to each offline simulation test; then constructing a state weighting coefficient matrix based on the lateral error weight coefficient, the lateral error rate weight coefficient, the heading error weight coefficient, and the heading error rate weight coefficient, and constructing an output weighting coefficient matrix based on the steering wheel angle weight coefficient, the state weighting coefficient matrix being a diagonal matrix, specifically, the lateral error weight coefficient, the lateral error rate weight coefficient, the heading error weight coefficient, and the heading error rate weight coefficient being non-zero values ​​in the first to fourth columns of the state weighting coefficient matrix, respectively, and the output weighting coefficient matrix being a 1×1 matrix;

[0068] By way of example and not limitation, since parking is generally performed at a low speed, the first preset vehicle speed may be 0-3 m / s, preferably 2 m / s.

[0069] The state weighting coefficient matrix and output weighting coefficient matrix that meet the first preset conditions are screened out from multiple simulation tests as the final state weighting coefficient matrix and the final output weighting coefficient matrix respectively; the first preset condition is that the lateral error is less than 10 cm and the heading error is less than 1°.

[0070] This is an example but not a limitation. In order to eliminate the gap between the simulation world and the real world, the three sets of parameters obtained are saved in a high-performance computing platform and manually calibrated and verified by a vehicle equipped with a high-performance computing platform. If the final parking state deviates from the target state by a small amount (the lateral error is less than 20cm and the heading error is less than 3°), the three sets of Q and R values ​​are calibrated and fine-tuned to obtain the three sets of Q and R matrices with the best parking effect under two different parking trajectory conditions (the lateral error is less than 10cm and the heading error is less than 1°). If, during the verification process, the final parking state deviates from the ideal state by a large amount (the lateral error is greater than 20cm or the heading error is greater than 3°), it is considered that the simulation calculation results do not meet the requirements, and the simulation steps are repeated, the Q and R values ​​are adjusted and simulated again to obtain new simulation calculation results until the Q and R matrices meet the first preset conditions.

[0071] It should be noted that each parking trajectory type corresponds to a Q and R matrix, as shown below:

[0072] (a) Optimal Q1 and R1 values ​​for vehicle backing up in one-stage parking;

[0073] (b) Optimal Q2 and R2 values ​​for the vehicle moving forward scenario in multi-stage parking;

[0074] (c) Optimal Q3 and R3 values ​​for the vehicle reversing scenario in multi-stage parking.

[0075] Furthermore, to calculate the key parameter matrix K, it is necessary to first iteratively calculate and solve P. However, iterative calculation consumes a lot of computing resources and is not suitable for real-time systems. Therefore, this embodiment adopts an offline calculation method to solve the K value through external resources and keep the K value in the domain controller to achieve the purpose of saving computing resources. The maximum number of iterations is set, specifically: set the maximum iteration error, let P n =Q,P n-1 =Q+A T P n (R+BR -1 B T P n ) -1 A, iterate the loop until P k+1 -P k If the deviation value is less than the set maximum iteration error, the iteration stops and the output P=P k ;

[0076] The key parameter matrix is ​​calculated according to the following formula:

[0077] K=-(R+B T PB) -1 B T PA

[0078] Among them, P nrepresents the matrix of the first iteration, P n-1 represents the matrix of the second iteration, P k+1 represents the matrix obtained at the k+1th iteration, P k represents the matrix obtained by the kth iteration, Q represents the final state weighting coefficient matrix, R represents the final output weighting coefficient matrix, A represents the state coefficient matrix, B represents the input control quantity matrix, T represents the transpose of the matrix, and K represents the key parameter matrix;

[0079] Based on the obtained key parameter matrices corresponding to one-stage reverse parking, multi-stage forward parking, and multi-stage reverse parking, a preset database is constructed and pre-stored in the target vehicle. Specifically, in an external computing resource, an offline K calculation formula is constructed, and three sets of A, B, Q, and R matrices are input. Offline iterative calculations of three sets of K values ​​are performed to obtain the optimal K1 for one-stage reverse parking, the optimal K2 for multi-stage forward parking, and the optimal K3 for multi-stage reverse parking, thereby constructing a preset database corresponding to the target vehicle.

[0080] The state coefficient matrix A is calculated according to the following formula:

[0081]

[0082] Among them, C f and C r They represent the front and rear wheel cornering stiffness of the target vehicle, m represents the vehicle mass, l f Indicates the length from the front end of the target vehicle to the center of mass, l r represents the length from the rear end of the target vehicle to the center of mass, v x represents the longitudinal velocity of the target vehicle, v y represents the lateral speed of the target vehicle, I z Represents the target vehicle's moment of inertia around the z-axis.

[0083] Furthermore, the input control matrix B is calculated according to the following formula:

[0084]

[0085] Among them, l f represents the length from the front end of the target vehicle to the center of mass, C f represents the front wheel cornering stiffness of the target vehicle, m represents the vehicle mass of the target vehicle, I z Represents the target vehicle's moment of inertia around the z-axis.

[0086] Step S03: Acquire the actual position information of the target vehicle, wherein the actual position information includes the actual coordinate information, actual heading information, and actual yaw angular velocity information of the target vehicle;

[0087] Step S04: selecting a target point from all trajectory points, and calculating a lateral error, a lateral error rate, a heading error, and a heading error rate based on the trajectory point information corresponding to the target point and the actual heading information, and constructing a state matrix based on the lateral error, the lateral error rate, the heading error, and the heading error rate, so as to calculate the front wheel turning angle of the target vehicle based on the state matrix and the key parameter matrix;

[0088] In some optional embodiments of the present invention, when selecting a target point, it is necessary to obtain the distance between each coordinate point and the target vehicle based on the coordinate information of each trajectory point and the actual coordinate information of the target vehicle, and select the coordinate point with the shortest distance from the distances between all coordinate points and the target vehicle as the target point.

[0089] Specifically, according to the current position of the vehicle, the vehicle position (x, y) and each trajectory point (x t ,y t ) and select the closest trajectory point as the target point.

[0090] For example, the distance between the vehicle and the first trajectory point is:

[0091]

[0092] The distance between the vehicle and the second trajectory point is:

[0093]

[0094] Similarly, the distance between the vehicle and the last trajectory point is (assuming the planned trajectory contains n trajectory points):

[0095]

[0096] If the shortest distance d min =min(d1, d2, ..., dn) = dr, then the rth point is the target point.

[0097] After accurately selecting the target point, the lateral error, lateral error rate, heading error, and heading error rate are calculated according to the following formula:

[0098]

[0099] Among them, e d Indicates lateral error, Indicates the lateral error rate, Indicates heading error, represents the heading error rate, v represents the target speed, x and y represent the horizontal and vertical coordinates in the actual coordinate information respectively, x rand y r Respectively represent the horizontal and vertical coordinates in the target point coordinate information, represents the target yaw rate, Indicates the vehicle target heading information. Indicates the actual heading information. Indicates the actual yaw rate information.

[0100] Then construct the state matrix according to the following formula:

[0101]

[0102] x(t) represents the state matrix corresponding to the target point t;

[0103] The front wheel angle of the target vehicle is calculated according to the following formula:

[0104] u(t)=-Kx(t)

[0105] Where u(t) represents the front wheel turning angle of the target vehicle at the target point t

[0106] Step S05: Determine whether the target vehicle has reached the end point of the trajectory. If the target vehicle has not reached the end point of the trajectory, control the target vehicle to park according to the front wheel angle of the target vehicle.

[0107] It is understandable that this embodiment greatly reduces the calculation delay compared to traditional calculations, but it cannot completely overcome the delay because no matter which solution is used, there will be more or less calculation delay. Based on this, in this step, the purpose of judging whether the target vehicle has reached the end point of the trajectory is to confirm whether the target vehicle has completed parking. If the target vehicle has not reached the end point of the trajectory, it means that the target vehicle has not completed parking. In this way, the target vehicle is controlled to park according to the calculated front wheel angle. In this way, when the actual position of the vehicle changes, a new parking trajectory is repeatedly obtained, and then a new front wheel angle is calculated until the target vehicle is detected to have reached the parking end point, thereby realizing trajectory tracking control of automatic parking at low speed.

[0108] In summary, according to the above-mentioned automatic parking trajectory tracking control method, the LQR optimization control strategy is adopted to calculate the target steering wheel angle (front wheel angle). Offline iterative calculation is used for the calculation of the key parameter matrix, and the calculation results are saved locally. The LQR iterative calculation is completed offline, avoiding the LQR iteration's requirements for the computing performance of the vehicle-side domain controller, enabling it to run in a low-computing real-time system, thereby greatly reducing the calculation delay and improving the parking control accuracy. Specifically, when the target vehicle begins to park, the parking trajectory is actively received, and the corresponding key parameter matrix is ​​quickly retrieved according to the type of parking trajectory to reduce calculations. Then, the actual position information of the target vehicle is obtained, and the nearest target point is selected to construct a state matrix based on the trajectory point information and actual heading information of the target point. The front wheel angle is then calculated based on the retrieved key parameter matrix and the state matrix. Then, it is determined whether the target vehicle has completed parking. If not, the target vehicle is controlled to park according to the front wheel angle, thereby greatly improving the control accuracy and solving the serious calculation delay problem of traditional technology.

[0109] See also Figure 2 , which is a schematic structural diagram of an automatic parking trajectory tracking control system in a second embodiment of the present invention, the system includes:

[0110] a parking trajectory acquisition module 10 for receiving a parking trajectory after the target vehicle starts parking, the parking trajectory comprising a plurality of trajectory points, and extracting trajectory point information of each trajectory point from the parking trajectory, the trajectory point information including coordinate information, vehicle body target heading information, target yaw angular velocity information, target speed information, target acceleration information, and road curvature;

[0111] a key parameter matrix retrieving module 20 for obtaining a parking trajectory type according to the parking trajectory, and retrieving a corresponding key parameter matrix from a preset database according to the parking trajectory type;

[0112] An actual position information acquisition module 30 is used to acquire the actual position information of the target vehicle, wherein the actual position information includes the actual coordinate information, actual heading information and actual yaw angular velocity information of the target vehicle;

[0113] a front wheel steering angle calculation module 40, configured to select a target point from all trajectory points, and calculate a lateral error, a lateral error rate, a heading error, and a heading error rate based on trajectory point information corresponding to the target point and the actual heading information, and construct a state matrix based on the lateral error, the lateral error rate, the heading error, and the heading error rate, so as to calculate the front wheel steering angle of the target vehicle based on the state matrix and the key parameter matrix;

[0114] Furthermore, the front wheel angle calculation module 40 further includes:

[0115] A target point selection unit is used to obtain the distance between each coordinate point and the target vehicle based on the coordinate information of each trajectory point and the actual coordinate information of the target vehicle, and select the coordinate point with the shortest distance from all the distances between the coordinate points and the target vehicle as the target point;

[0116] The first calculation unit is configured to calculate the lateral error, the lateral error rate, the heading error, and the heading error rate according to the following formula:

[0117]

[0118] Among them, e d Indicates lateral error, Indicates the lateral error rate, Indicates heading error, represents the heading error rate, v represents the target speed, x and y represent the horizontal and vertical coordinates in the actual coordinate information respectively, x r and y r Respectively represent the horizontal and vertical coordinates in the target point coordinate information, represents the target yaw rate, Indicates the vehicle target heading information. Indicates the actual heading information. Indicates the actual yaw rate information;

[0119] The state matrix construction unit is used to construct the state matrix according to the following formula:

[0120]

[0121] x(t) represents the state matrix corresponding to the target point t;

[0122] The second calculation unit is used to calculate the front wheel turning angle of the target vehicle according to the following formula:

[0123] u(t)=-Kx(t)

[0124] Where u(t) represents the front wheel turning angle of the target vehicle at the target point t.

[0125] The parking control module 50 is used to determine whether the target vehicle has reached the end of the trajectory. If the target vehicle has not reached the end of the trajectory, the target vehicle is parked according to the front wheel angle of the target vehicle;

[0126] Furthermore, in some optional embodiments of the present invention, the system includes:

[0127] The system further comprises:

[0128] a simulation test module, configured to control a target vehicle at a first preset vehicle speed to perform multiple offline simulation tests for each parking trajectory type, where the parking trajectory types include single-stage reverse parking and multi-stage parking, where the multi-stage parking includes multi-stage forward parking and multi-stage reverse parking, to obtain a lateral error weight coefficient, a lateral error rate weight coefficient, a heading error weight coefficient, a heading error rate weight coefficient, and a steering wheel angle weight coefficient corresponding to each offline simulation test;

[0129] A weighted coefficient matrix construction module is used to construct a state weighted coefficient matrix based on the lateral error weight coefficient, the lateral error rate weight coefficient, the heading error weight coefficient, and the heading error rate weight coefficient, and to construct an output weighted coefficient matrix based on the steering wheel angle weight coefficient. The state weighted coefficient matrix is ​​a diagonal matrix.

[0130] Screening out a state weighting coefficient matrix and an output weighting coefficient matrix that meet the first preset condition from multiple simulation tests as the final state weighting coefficient matrix and the final output weighting coefficient matrix respectively;

[0131] The loop iteration module is used to set the maximum number of iterations and the maximum iteration error. n =Q,P n-1 =Q+A T P n (R+BR -1 B T P n ) -1 A, iterate the loop until P k+1 -P k If the deviation value is less than the set maximum iteration error, the iteration stops and the output P=P k ;

[0132] The key parameter calculation module is used to calculate the key parameter matrix according to the following formula:

[0133] K=-(R+B T PB) -1 B T PA

[0134] Among them, P n represents the matrix of the first iteration, P n-1 represents the matrix of the second iteration, P k+1 represents the matrix obtained at the k+1th iteration, P k represents the matrix obtained by the kth iteration, Q represents the final state weighting coefficient matrix, R represents the final output weighting coefficient matrix, A represents the state coefficient matrix, B represents the input control quantity matrix, T represents the transpose of the matrix, and K represents the key parameter matrix;

[0135] Furthermore, the key parameter calculation module also includes:

[0136] The third calculation unit is used to calculate the state coefficient matrix according to the following formula:

[0137]

[0138] Among them, C f and C r They represent the front and rear wheel cornering stiffness of the target vehicle, m represents the vehicle mass, l f Indicates the length from the front end of the target vehicle to the center of mass, l r represents the length from the rear end of the target vehicle to the center of mass, v x represents the longitudinal velocity of the target vehicle, v y represents the lateral velocity of the target vehicle, I z represents the moment of inertia of the target vehicle around the z-axis;

[0139] The fourth calculation unit is used to calculate the input control quantity matrix according to the following formula:

[0140]

[0141] Among them, l f represents the length from the front end of the target vehicle to the center of mass, C f represents the front wheel cornering stiffness of the target vehicle, m represents the vehicle mass of the target vehicle, I z Represents the target vehicle's moment of inertia around the z-axis.

[0142] The preset database construction module is used to construct a preset database based on the obtained key parameter matrices corresponding to one-stage reverse parking, multi-stage forward parking and multi-stage reverse parking, and pre-store the preset database in the target vehicle.

[0143] In summary, according to the above-mentioned automatic parking trajectory tracking control system, the LQR optimization control strategy is adopted to calculate the target steering wheel angle (front wheel angle). Offline iterative calculation is adopted for the calculation of the key parameter matrix, and the calculation results are saved locally. The LQR iterative calculation is completed offline, avoiding the LQR iteration's requirements for the computing performance of the vehicle-side domain controller, so that it can run in a real-time system with low computing power, thereby greatly reducing the calculation delay and improving the parking control accuracy. Specifically, when the target vehicle starts to park, the parking trajectory is actively received, and the corresponding key parameter matrix is ​​quickly retrieved according to the type of parking trajectory to reduce calculations. Then, the actual position information of the target vehicle is obtained, and the nearest target point is selected to construct a state matrix based on the trajectory point information and actual heading information of the target point. The front wheel angle is then calculated based on the retrieved key parameter matrix and the state matrix. Then, it is determined whether the target vehicle has completed parking. If not, the target vehicle is controlled to park according to the front wheel angle, thereby greatly improving the control accuracy and solving the serious calculation delay problem existing in traditional technologies.

[0144] Throughout this specification, reference to terms such as "one embodiment," "some embodiments," "examples," "specific examples," or "some examples" means that a specific feature, structure, material, or characteristic described in conjunction with that embodiment or example is included in at least one embodiment or example of the present invention. In this specification, schematic representations of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.

[0145] The above-described embodiments merely illustrate several implementations of the present invention, and while their descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the present invention. It should be noted that a person skilled in the art would be able to make numerous variations and improvements without departing from the spirit of the present invention, all of which fall within the scope of protection of the present invention. Therefore, the scope of protection of the present invention shall be determined by the appended claims.

Claims

1. An automatic parking trajectory tracking control method, characterized in that: The method comprises: After the target vehicle starts parking, a parking trajectory is received, the parking trajectory comprising a plurality of trajectory points, and trajectory point information of each trajectory point is extracted from the parking trajectory, the trajectory point information comprising coordinate information, vehicle body target heading information, target yaw angular velocity information, target speed information, target acceleration information, and road curvature; Obtaining a parking trajectory type according to the parking trajectory, and retrieving a corresponding key parameter matrix from a preset database according to the parking trajectory type; Acquiring actual position information of the target vehicle positioning, wherein the actual position information includes actual coordinate information, actual heading information, and actual yaw angular velocity information of the target vehicle; Selecting a target point from all trajectory points, and calculating a lateral error, a lateral error rate, a heading error, and a heading error rate based on trajectory point information corresponding to the target point and the actual heading information, and constructing a state matrix based on the lateral error, the lateral error rate, the heading error, and the heading error rate, so as to calculate a front wheel turning angle of the target vehicle based on the state matrix and the key parameter matrix; Determine whether the target vehicle has reached the end of the trajectory. If the target vehicle has not reached the end of the trajectory, control the target vehicle to park according to the front wheel angle of the target vehicle.

2. The automatic parking trajectory tracking control method according to claim 1, characterized in that: The steps of constructing the preset database include: Controlling the target vehicle at a first preset speed to perform multiple offline simulation tests for each parking trajectory type, where the parking trajectory types include single-stage reverse parking and multi-stage parking, where the multi-stage parking includes multi-stage forward parking and multi-stage reverse parking, to obtain a lateral error weight coefficient, a lateral error rate weight coefficient, a heading error weight coefficient, a heading error rate weight coefficient, and a steering wheel angle weight coefficient corresponding to each offline simulation test; Constructing a state weighting coefficient matrix according to the lateral error weight coefficient, the lateral error rate weight coefficient, the heading error weight coefficient, and the heading error rate weight coefficient; constructing an output weighting coefficient matrix according to the steering wheel angle weight coefficient, wherein the state weighting coefficient matrix is ​​a diagonal matrix; Screening out a state weighting coefficient matrix and an output weighting coefficient matrix that meet the first preset condition from multiple simulation tests as the final state weighting coefficient matrix and the final output weighting coefficient matrix respectively; Set the maximum number of iterations, set the maximum iteration error, and let P n =Q,P n-1 =Q+A T P n (R+BR -1 B T P n ) -1 A, iterate the loop until P k+1 -P k If the deviation value is less than the set maximum iteration error, the iteration stops and the output P=P k ; The key parameter matrix is ​​calculated according to the following formula: K=-(R+B T PB) -1 B T PA Among them, P n represents the matrix of the first iteration, P n-1 represents the matrix of the second iteration, P k+1 represents the matrix obtained by the k+1th iteration, P k represents the matrix obtained by the kth iteration, Q represents the final state weighting coefficient matrix, R represents the final output weighting coefficient matrix, A represents the state coefficient matrix, B represents the input control quantity matrix, T represents the transpose of the matrix, and K represents the key parameter matrix; A preset database is constructed based on the obtained key parameter matrices corresponding to the one-stage reverse parking, the multi-stage forward parking, and the multi-stage reverse parking, and the preset database is pre-stored in the target vehicle.

3. The automatic parking trajectory tracking control method according to claim 2, characterized in that: The step of selecting a target point from all trajectory points comprises: The distance between each coordinate point and the target vehicle is obtained according to the coordinate information of each trajectory point and the actual coordinate information of the target vehicle, and the coordinate point with the shortest distance from all the distances between the coordinate points and the target vehicle is selected as the target point.

4. The automatic parking trajectory tracking control method according to claim 3, characterized in that: The step of calculating the lateral error, the lateral error rate, the heading error, and the heading error rate based on the trajectory point information corresponding to the target point and the actual heading information comprises: The lateral error, lateral error rate, heading error, and heading error rate are calculated using the following formulas: Among them, e d Indicates lateral error, Indicates the lateral error rate, Indicates heading error, represents the heading error rate, v represents the target speed, x and y represent the horizontal and vertical coordinates in the actual coordinate information respectively, x r and y r Respectively represent the horizontal and vertical coordinates in the target point coordinate information, represents the target yaw rate, Indicates the vehicle target heading information. Indicates the actual heading information. Indicates the actual yaw rate information.

5. The automatic parking trajectory tracking control method according to claim 4, characterized in that: The step of constructing a state matrix according to the lateral error, the lateral error rate, the heading error, and the heading error rate, and calculating the front wheel turning angle of the target vehicle according to the state matrix and the key parameter matrix includes: The state matrix is ​​constructed according to the following formula: x(t) represents the state matrix corresponding to the target point t; The front wheel angle of the target vehicle is calculated according to the following formula: u(t)=-Kx(t) Where u(t) represents the front wheel turning angle of the target vehicle at the target point t.

6. The automatic parking trajectory tracking control method according to claim 2, characterized in that: The state coefficient matrix is ​​calculated according to the following formula: Among them, C f and C r They represent the front and rear wheel cornering stiffness of the target vehicle, m represents the vehicle mass, l f Indicates the length from the front end of the target vehicle to the center of mass, l r represents the length from the rear end of the target vehicle to the center of mass, v x represents the longitudinal velocity of the target vehicle, v y represents the lateral velocity of the target vehicle, I z Represents the target vehicle's moment of inertia around the z-axis.

7. The automatic parking trajectory tracking control method according to claim 2, characterized in that: The input control matrix is ​​calculated according to the following formula: Among them, l f represents the length from the front end of the target vehicle to the center of mass, C f represents the front wheel cornering stiffness of the target vehicle, m represents the vehicle mass of the target vehicle, I z Represents the target vehicle's moment of inertia around the z-axis.

8. An automatic parking trajectory tracking control system, characterized in that: The system comprises: a parking trajectory acquisition module, configured to receive a parking trajectory after the target vehicle begins parking, the parking trajectory comprising a plurality of trajectory points, and extract trajectory point information of each trajectory point from the parking trajectory, the trajectory point information including coordinate information, vehicle body target heading information, target yaw angular velocity information, target speed information, target acceleration information, and road curvature; a key parameter matrix retrieving module, configured to obtain a parking trajectory type according to the parking trajectory, and retrieve a corresponding key parameter matrix from a preset database according to the parking trajectory type; An actual position information acquisition module is used to acquire the actual position information of the target vehicle, wherein the actual position information includes the actual coordinate information, actual heading information and actual yaw angular velocity information of the target vehicle; a front wheel steering angle calculation module, configured to select a target point from all trajectory points, and calculate a lateral error, a lateral error rate, a heading error, and a heading error rate based on trajectory point information corresponding to the target point and the actual heading information, and construct a state matrix based on the lateral error, the lateral error rate, the heading error, and the heading error rate, so as to calculate the front wheel steering angle of the target vehicle based on the state matrix and the key parameter matrix; The parking control module is used to determine whether the target vehicle has reached the end of the trajectory. If the target vehicle has not reached the end of the trajectory, the target vehicle is controlled to park according to the front wheel angle of the target vehicle.

9. The automatic parking trajectory tracking control system according to claim 8, characterized in that: The system further comprises: a simulation test module, configured to control a target vehicle at a first preset vehicle speed to perform multiple offline simulation tests for each parking trajectory type, where the parking trajectory types include single-stage reverse parking and multi-stage parking, where the multi-stage parking includes multi-stage forward parking and multi-stage reverse parking, to obtain a lateral error weight coefficient, a lateral error rate weight coefficient, a heading error weight coefficient, a heading error rate weight coefficient, and a steering wheel angle weight coefficient corresponding to each offline simulation test; A weighted coefficient matrix construction module is used to construct a state weighted coefficient matrix based on the lateral error weight coefficient, the lateral error rate weight coefficient, the heading error weight coefficient, and the heading error rate weight coefficient, and to construct an output weighted coefficient matrix based on the steering wheel angle weight coefficient. The state weighted coefficient matrix is ​​a diagonal matrix. Screening out a state weighting coefficient matrix and an output weighting coefficient matrix that meet the first preset condition from multiple simulation tests as the final state weighting coefficient matrix and the final output weighting coefficient matrix respectively; The loop iteration module is used to set the maximum number of iterations and the maximum iteration error. n =Q,P n-1 =Q+A T P n (R+BR -1 B T P n ) -1 A, iterate the loop until P k+1 -P k If the deviation value is less than the set maximum iteration error, the iteration stops and the output P=P k ; The key parameter calculation module is used to calculate the key parameter matrix according to the following formula: K=-(R+B T PB) -1 B T PA Among them, P n represents the matrix of the first iteration, P n-1 represents the matrix of the second iteration, P k+1 represents the matrix obtained by the k+1th iteration, P k represents the matrix obtained by the kth iteration, Q represents the final state weighting coefficient matrix, R represents the final output weighting coefficient matrix, A represents the state coefficient matrix, B represents the input control quantity matrix, T represents the transpose of the matrix, and K represents the key parameter matrix; The preset database construction module is used to construct a preset database based on the obtained key parameter matrices corresponding to one-stage reverse parking, multi-stage forward parking and multi-stage reverse parking, and pre-store the preset database in the target vehicle.

10. The automatic parking trajectory tracking control system according to claim 8, characterized in that: The front wheel angle calculation module also includes: The target point selection unit is used to obtain the distance between each coordinate point and the target vehicle based on the coordinate information of each trajectory point and the actual coordinate information of the target vehicle, and to select the coordinate point with the shortest distance from all the distances between the coordinate points and the target vehicle as the target point.

Images