Vehicle trajectory automatic tracking method based on model predictive control system

Abstract

The invention provides a vehicle trajectory automatic tracking method based on a model predictive control system, which comprises the following steps of: optimizing a state weight matrix and an output weight matrix by utilizing a sparrow algorithm based on a current vehicle speed, a current vehicle pose, a reference trajectory, the model predictive control system and a fitness function; and obtaining an optimal state weight matrix and an optimal output weight matrix. And inputting the current vehicle speed, the current vehicle pose, the reference trajectory, the optimal state weight matrix and the optimal output weight matrix into a model prediction control system, calculating to obtain an optimal steering angle, and automatically controlling front wheels of the vehicle to be adjusted to the optimal steering angle. Based on the scheme, the optimal state weight matrix and the optimal output weight matrix can be automatically searched without depending on experience, so that when the model predictive control system automatically controls the steering of the front wheels of the vehicle, the vehicle can be slowly close to the reference trajectory, and shaking caused by repeated left-right trajectory correction is avoided.

Description

TECHNICAL FIELD

[0001] The present application generally relates to the field of vehicle trajectory tracking, and particularly relates to a vehicle trajectory automatic tracking method based on a model predictive control system. BACKGROUND

[0002] The model predictive control system (MPC) is an advanced control strategy widely used in industrial process control, robot control, automatic driving and other fields. The core of MPC is to use the system model to predict the output of the system in the future period of time.

[0003] In the field of automatic driving, MPC can calculate the steering angle of the front wheel of the vehicle according to the current vehicle speed, the current vehicle pose, the set trajectory, the state weight matrix and the output weight matrix, and then control the steering of the front wheel of the vehicle according to the steering angle.

[0004] When there is a deviation between the current vehicle pose and the set trajectory, the steering angle of the front wheel of the vehicle can be calculated according to the current vehicle speed, the current vehicle pose, the set trajectory, and the set state weight matrix and output weight matrix, so that the vehicle can gradually approach and travel along the set trajectory.

[0005] The setting of the state weight matrix and the output weight matrix has a great influence on the calculation of the steering angle by MPC, and the existing technology still relies on experience to determine. Once the setting of the state weight matrix and the output weight matrix is unreasonable, it may cause excessive adjustment of MPC in automatically controlling the steering of the front wheel of the vehicle, and then cause the vehicle to repeatedly correct the trajectory left and right, resulting in obvious shaking. SUMMARY

[0006] In view of the above defects or deficiencies in the prior art, it is desirable to provide a vehicle trajectory automatic tracking method based on a model predictive control system.

[0007] The present application provides a vehicle trajectory automatic tracking method based on a model predictive control system, comprising: S1: obtaining a current vehicle speed, a current vehicle pose and a reference trajectory; the reference trajectory is a pre-set driving trajectory of the vehicle; S2: constructing a fitness function according to the current vehicle pose and the reference trajectory; S3: updating the model predictive control system; the input of the model predictive control system is the current vehicle speed, the current vehicle pose, the reference trajectory, the state weight matrix and the output weight matrix, and the output is the steering angle; the model predictive control system is also used to automatically control the vehicle to adjust the front wheel according to the steering angle; S4: based on the current vehicle speed, the current vehicle pose, the reference trajectory, the model predictive control system and the fitness function, the state weight matrix and the output weight matrix are optimized by using the sparrow algorithm to obtain the optimal state weight matrix and the optimal output weight matrix; S5: inputting the current vehicle speed, the current vehicle pose, the reference trajectory, the optimal state weight matrix and the optimal output weight matrix into the model predictive control system, and calculating to obtain the optimal steering angle, and automatically controlling the front wheel of the vehicle to adjust to the optimal steering angle; S6: repeating steps S1 to S6 every interval setting period.

[0008] According to the technical scheme provided by the application, the updating model predictive control system comprises: S3-1: obtaining an initial model predictive control system; the initial model predictive control system has a prediction time domain parameter; the initial model predictive control system calculates a steering angle according to the prediction time domain parameter, the current vehicle speed, the current vehicle pose, the reference trajectory, the state weight matrix and the output weight matrix; S3-2: based on the current vehicle speed, the current vehicle pose, the reference trajectory, the initial model predictive control system and the fitness function, the prediction time domain parameter is optimized by using the sparrow algorithm to obtain the optimized prediction time domain parameter; S3-3: substituting the optimized prediction time domain parameter into the initial model predictive control system to obtain the model predictive control system.

[0009] According to the technical scheme provided by the application, the updating model predictive control system comprises: S3-2-1: obtaining the initial state weight matrix and the initial output weight matrix of the model predictive control system currently used by the vehicle; S3-2-2: setting a first search range of the prediction time domain parameter and a first iteration parameter of the sparrow algorithm; S3-2-3: according to the first search range and the first iteration parameter, the iteration prediction time domain parameter corresponding to all sparrows in the sparrow algorithm is obtained; S3-2-4: substituting the iteration prediction time domain parameter into the initial model predictive control system to obtain an iteration model predictive control system; S3-2-5: based on the iteration model predictive control system, a plurality of first iteration steering angles and a plurality of first iteration vehicle poses are calculated according to the current vehicle speed, the current vehicle pose, the reference trajectory, the initial state weight matrix and the initial output weight matrix; S3-2-6: calculating a first iteration fitness according to the plurality of first iteration vehicle poses, the reference trajectory and the fitness function; S3-2-7: when the end condition of the sparrow algorithm is reached, taking the iteration predicted time domain parameter corresponding to the historical maximum value in the plurality of first iteration fitnesses as the optimized predicted time domain parameter; otherwise, updating the positions of all sparrows in the first search range and the first iteration parameters, and repeating steps S3-2-3 to S3-2-7.

[0010] According to the technical scheme provided by the present application, the current vehicle speed, the current vehicle pose, the reference trajectory, the model predictive control system and the fitness function are used to optimize the state weight matrix and the output weight matrix by using the sparrow algorithm, and the optimal state weight matrix and the optimal output weight matrix are obtained, which comprises: S4-1: setting a second search range of the state weight matrix and the output weight matrix, and a second iteration parameter of the sparrow algorithm; S4-2: calculating the iteration state weight matrix and the iteration output weight matrix corresponding to all sparrows in the sparrow algorithm according to the second search range and the second iteration parameter; S4-3: calculating a plurality of second iteration steering angles and a plurality of second iteration vehicle poses based on the model predictive control system according to the current vehicle speed, the current vehicle pose, the reference trajectory, the iteration state weight matrix and the iteration output weight matrix; S4-4: calculating a second iteration fitness according to the plurality of second iteration vehicle poses, the reference trajectory and the fitness function; S4-5: when the end condition of the sparrow algorithm is reached, taking the state weight matrix and the output weight matrix corresponding to the historical maximum value in the plurality of second iteration fitnesses as the optimal state weight matrix and the optimal output weight matrix; otherwise, updating the positions of all sparrows in the second search range and the second iteration parameters, and repeating steps S4-2 to S4-5.

[0011] According to the technical scheme provided by the present application, the second iteration parameter comprises: the initial position of the sparrow, the identity of the sparrow; the identity of the sparrow comprises: the discoverer, the follower and the guard; The updating of the second iteration parameter comprises: updating the position of the sparrow according to the current identity of the sparrow; redistributing the identity of the sparrow according to the second iteration fitness corresponding to the position of the sparrow in the last iteration.

[0012] According to the technical scheme provided by the present application, the proportion of the number of discoverer sparrows to the total number of sparrows monotonically decreases with the increase of the iteration number.

[0013] According to the technical scheme provided in the application, the model predictive control system is used to calculate a plurality of second iteration steering angles and a plurality of second iteration vehicle poses based on the current vehicle speed, the current vehicle pose, the reference trajectory, the iteration state weight matrix and the iteration output weight matrix, and the method comprises the following steps: S4-3-1: inputting the current vehicle speed, the current vehicle pose, the reference trajectory, the iteration state weight matrix and the iteration output weight matrix into the model predictive control system to obtain a second iteration steering angle; S4-3-2: updating the second iteration vehicle pose according to the current second iteration vehicle pose, the current vehicle speed and the latest obtained second iteration steering angle; the current second iteration vehicle pose is initially the current vehicle pose, and the subsequent second iteration vehicle pose is the second iteration vehicle pose obtained after the last update; S4-3-3: repeating steps S4-3-1 to S4-3-3 until the total number of second iteration vehicle poses reaches a first set value.

[0014] According to the technical scheme provided in the application, the second iteration fitness is calculated based on the plurality of second iteration vehicle poses, the reference trajectory and the fitness function, and the method comprises the following steps: calculating a plurality of minimum distances from the plurality of second iteration vehicle poses to the reference trajectory; substituting all the minimum distances into the fitness function to calculate the second iteration fitness.

[0015] According to the technical scheme provided in the application, the reference trajectory comprises a plurality of preset position points; The method for calculating the minimum distances from the plurality of second iteration vehicle poses to the reference trajectory comprises the following steps: calculating a first distance between the current vehicle pose and all the preset position points; setting a search radius according to the current vehicle speed, a set period and the minimum first distance; taking the preset position points in a circle with the current vehicle pose as the center and the search radius as the radius as reference preset position points; calculating distances between all the reference preset position points and all the second iteration vehicle poses to obtain a plurality of second distances; taking the minimum second distances corresponding to the plurality of second iteration vehicle poses as the plurality of minimum distances, respectively.

[0016] The application has the following beneficial effects: The fitness function is constructed according to the current vehicle pose and the reference trajectory, the state weight matrix and the output weight matrix are optimized by using the sparrow algorithm based on the current vehicle speed, the current vehicle pose, the reference trajectory, the model predictive control system and the fitness function, and the optimal state weight matrix and the optimal output weight matrix are obtained. The current vehicle speed, the current vehicle pose, the reference trajectory, the optimal state weight matrix and the optimal output weight matrix are input into the model predictive control system, the optimal steering angle is calculated, and the front wheel of the vehicle is automatically controlled to adjust to the optimal steering angle. Based on the above scheme, the optimal state weight matrix and the output weight matrix can be automatically searched without relying on experience, so that the model predictive control system can make the vehicle gently approach the reference trajectory when automatically controlling the front wheel steering of the vehicle, and avoid shaking caused by repeatedly correcting the trajectory left and right. BRIEF DESCRIPTION OF DRAWINGS

[0017] Other features, objects and advantages of the present application will become more apparent from the following detailed description of non-limiting embodiments thereof, read in conjunction with the accompanying drawings: Figure 1 A flowchart of a vehicle trajectory automatic tracking method based on a model predictive control system. DETAILED DESCRIPTION

[0018] The present application will be further described below in conjunction with the drawings and embodiments. It should be understood that the specific embodiments described herein are merely intended to explain the related application, and not to limit the application. In addition, it should be noted that only the parts related to the application are shown in the drawings for ease of description.

[0019] It should be noted that the embodiments in the present application and the features in the embodiments can be combined with each other without conflict. The present application will be described in detail below with reference to the drawings and in conjunction with the embodiments.

[0020] Reference Figure 1 The present application provides a vehicle trajectory automatic tracking method based on a model predictive control system, comprising: S1: obtaining a current vehicle speed, a current vehicle pose and a reference trajectory; the reference trajectory is a pre-set driving trajectory of the vehicle; In this embodiment, the current vehicle pose includes the position and the moving direction of the vehicle. The reference trajectory includes a plurality of discrete pre-set position points, and a pre-set moving direction and a trajectory curvature at each pre-set position point. The plurality of pre-set position points are sequentially connected to form the reference trajectory.

[0021] S2: constructing a fitness function according to the current vehicle pose and the reference trajectory; Wherein, the fitness function is a function for measuring the ability of individuals in the population to adapt to the environment. Specifically, the greater the fitness of the individual (sparrow in this embodiment) in the environment (first search range or second search range in this embodiment), the closer the individual is to the ideal result required by the algorithm.

[0022] In this embodiment, if a sparrow moves in the first search range, when it moves to a certain position, there is a combination of an iterative prediction horizon parameter. Another sparrow moves in the second search range, when it moves to a certain position, there is a combination of an iterative state weight matrix and an iterative output weight matrix.

[0023] Further, the iterative prediction horizon parameter is substituted into the initial model predictive control system, and a plurality of first iterative steering angles and a plurality of first iterative vehicle poses are calculated according to the current vehicle speed, the current vehicle pose, the reference trajectory, the initial state weight matrix and the initial output weight matrix; finally, the first iterative fitness is calculated according to the first iterative vehicle pose, the reference trajectory and the fitness function.

[0024] Or according to the current vehicle speed, the current vehicle pose, the reference trajectory, the iterative state weight matrix and the iterative output weight matrix, a plurality of second iterative steering angles and a plurality of second iterative vehicle poses are calculated, and the second iterative fitness is calculated in the same way.

[0025] Until the sparrow algorithm reaches the end condition, the iterative prediction horizon parameter corresponding to the historical maximum value in all first iterative fitnesses is taken as the optimal solution of the first sparrow algorithm.

[0026] Or the iterative state weight matrix and the iterative output weight matrix corresponding to the historical maximum value in all second iterative fitnesses are taken as the optimal solution of the second sparrow algorithm.

[0027] According to the fitness function, it can be judged which of the many schemes searched by the sparrow algorithm is closer to the optimal scheme in the ideal.

[0028] In this embodiment, the first sparrow algorithm and the second sparrow algorithm are only distinguished in name, and there is no essential difference between them; due to different optimization objectives, the data substituted in the search process is different, and the specific iteration method of the algorithm is exactly the same.

[0029] Specifically, the fitness function is:

[0030] Wherein, f represents the value of the iterative fitness (first iterative fitness or second iterative fitness in this embodiment); i =1, 2, 3... n ,n represents a first set value, which is 10 in this embodiment; q i represents the i th iteration vehicle pose (first iteration vehicle pose or second iteration vehicle pose in this embodiment); R represents a reference trajectory; represents the i th iteration vehicle pose to the minimum distance of the reference trajectory R .

[0031] In the application scenario of this embodiment, the vehicle needs to drive the trajectory after turning more closely to the reference trajectory, and the trajectory will not be repeatedly corrected left and right. Therefore, the turning angle calculated by the variables (the "prediction time domain parameter" or the "state weight matrix and output weight matrix") obtained by the Sparrow algorithm iteration is taken as the initial turning angle, and the trajectory is deduced for the first set value times with the current vehicle pose as the starting point. After completing the trajectory deduction for the first set value times, a plurality of first iteration vehicle poses are obtained. Finally, the minimum distances of the plurality of first iteration vehicle poses to the reference trajectory are calculated.

[0032] The smaller the sum of the plurality of minimum distances is, the greater the value of the fitness after being substituted into the fitness function, indicating that it is closer to the desired ideal solution.

[0033] Specifically, since the distance between two points can only be greater than or equal to 0; therefore, if you want to make the fitness function as large as possible, you need to make the sum of the plurality of minimum distances as small as possible.

[0034] Without considering other factors, the desired ideal solution is to make the sum of the plurality of minimum distances equal to 0, and then make the value of the fitness equal to 0. However, in actual situations, there are various factors that limit the sum of the plurality of minimum distances from being equal to 0. Therefore, it is necessary to use the Sparrow algorithm to search for an optimal solution that makes the fitness function as large as possible (the plurality of minimum distances as small as possible).

[0035] S3: updating a model predictive control system; the input of the model predictive control system is the current vehicle speed, the current vehicle pose, the reference trajectory, the state weight matrix and the output weight matrix, and the output is the turning angle; the model predictive control system is also used to automatically control the vehicle to adjust the front wheel according to the turning angle, comprising: S3-1: obtaining an initial model predictive control system; the initial model predictive control system has a prediction time domain parameter; the initial model predictive control system calculates the turning angle according to the prediction time domain parameter, and the current vehicle speed, the current vehicle pose, the reference trajectory, the state weight matrix and the output weight matrix; It should be noted that the model predictive control system is an existing control scheme, which can calculate the steering angle according to the prediction time domain parameter, the current vehicle speed, the current vehicle pose, the reference trajectory, the state weight matrix and the output weight matrix. However, in general, the prediction time domain parameter is a fixed value, and only the current vehicle speed, the current vehicle pose, the reference trajectory, the state weight matrix and the output weight matrix are needed to calculate the steering angle.

[0036] In this embodiment, it is considered that the prediction time domain parameter also has an impact on the calculation of the steering angle. Therefore, the prediction time domain parameter of the model predictive control system is optimized first, and then the state weight matrix and the output weight matrix are optimized by using the model predictive control system with the optimized prediction time domain parameter. From multiple angles, the calculation logic of MPC is optimized, so that the multiple steering angles obtained during the continuous control of the front wheels of the vehicle can make the vehicle turn more gently, and avoid repeated left and right steering adjustment of the trajectory, which causes unnecessary shaking.

[0037] S3-2: Based on the current vehicle speed, the current vehicle pose, the reference trajectory, the initial model predictive control system and the fitness function, the sparrow algorithm (as the first sparrow algorithm) is used to optimize the prediction time domain parameter to obtain the optimized prediction time domain parameter, including: S3-2-1: Obtain the initial state weight matrix and the initial output weight matrix of the model predictive control system currently used by the vehicle; S3-2-2: Set the first search range of the prediction time domain parameter and the first iteration parameter of the sparrow algorithm; In this embodiment, the first search range is set to [20, 50], and the prediction time domain parameter is initially 40.

[0038] The first iteration parameter includes: The number of sparrow populations, the initial position of the sparrow, the identity of the sparrow, and the maximum number of iterations (set to 100 generations in this embodiment); The identity of the sparrow includes: discoverer, follower and alarm. The main behavior of the discoverer is to search for a position with higher fitness in the search range where it is located; The follower will follow the discoverer and search around the discoverer to quickly search for a position with higher fitness; The alarm has a lower fitness at its own position, which will prompt the surrounding sparrows that there is danger at this position, so as to drive other sparrows away from this position.

[0039] Based on the above behavior, the discoverer can preferentially discover the position with higher fitness, the follower follows the discoverer and carefully searches around the discoverer to discover the position with higher fitness. The alarmer is in the position with lower fitness and drives away the sparrows around, which can accelerate the convergence of the sparrow population to the position with higher fitness, and the existence of the alarmer can also make the sparrow population avoid gathering at the local optimal position and have more opportunities to move towards the global optimal position.

[0040] Therefore, the sparrow algorithm can be applied to accurately predict the optimal prediction time domain parameter.

[0041] Specifically, the updating the first iteration parameter comprises: updating the position of the sparrow according to the current identity of the sparrow; the specific updating process is performed according to the discoverer position updating formula, the follower position updating formula and the alarmer position updating formula in the sparrow algorithm, and this process belongs to the prior art and will not be described here.

[0042] In this embodiment, the identity of the sparrow is re-assigned according to the first iteration fitness corresponding to the position of the sparrow in the last iteration.

[0043] Specifically, the population number of the sparrow is set to 100; the initial position of the sparrow is that the tent mapping is satisfied in the first search range; at the beginning, 40 sparrows are taken as discoverers, 50 sparrows are taken as followers, and 10 sparrows are taken as alarmers.

[0044] After the position updating and the calculation of the fitness of the position of each sparrow are completed, the 40 sparrows with the highest fitness are re-assigned as discoverers, the 10 sparrows with the lowest fitness are re-assigned as alarmers, and the remaining sparrows are re-assigned as followers.

[0045] In some embodiments, the proportion of the number of sparrows as discoverers to the total number of sparrows monotonically decreases with the increase of the number of iterations.

[0046] Specifically, the total number of discoverers decreases by 1 every 5 iterations.

[0047] According to the above identity assignment mode, after 5 iterations, when the identity is re-assigned, only the 39 sparrows with the highest fitness are assigned as discoverers; in addition, the 10 sparrows with the lowest fitness are still assigned as alarmers, and the remaining sparrows are assigned as followers, and at this time, the number of followers is 51.

[0048] By analogy, since the number of discoverers is initially 40 and decreases by 1 every 5 iterations, after 100 iterations, the number of discoverers is 20. During this process, the number of alarmers remains unchanged, and the number of followers increases.

[0049] In this way, the large number of discoverers in the early stages of iteration gives the sparrow population a strong global search capability, enabling it to find the global optimum (i.e., the position with the highest fitness) more quickly. As the number of iterations increases, the number of discoverers decreases, causing the sparrow population to become more clustered and eventually converge around the global optimum, thus increasing the probability of the sparrow population finding the global optimum.

[0050] S3-2-3: Based on the first search range and the first iteration parameters, obtain the iterative prediction time-domain parameters corresponding to all sparrows in the sparrow algorithm; Specifically, based on the sparrow's current identity (belonging to the first iteration parameter), the corresponding position is selected to update the formula and calculate the iterative prediction time-domain parameters within the first search range after iteration. Since the sparrow's location is actually a specific value within the first search range, the value of the iterative prediction time-domain parameters can be directly obtained here.

[0051] S3-2-4: Substitute the iterative prediction time-domain parameters into the initial model prediction control system to obtain the iterative model prediction control system; S3-2-5: Based on the iterative model prediction control system, multiple first-iteration steering angles and multiple first-iteration vehicle poses are calculated according to the current vehicle speed, current vehicle pose, reference trajectory, initial state weight matrix and initial output weight matrix; This process is the trajectory deduction described above, and the specific process includes: S3-2-5-1: Input the current vehicle speed, the vehicle pose derived from this trajectory, the reference trajectory, the initial state weight matrix, and the initial output weight matrix into the iterative model predictive control system to calculate the... i The first iteration turning angle; i Initially, it is 1; S3-2-5-2: According to the... i The first iteration steering angle and the vehicle pose derived from the current trajectory are used to calculate the... i The first iteration of vehicle pose; In this embodiment, a single trajectory calculation only calculates the vehicle's pose after one set period. It is known that the distance the vehicle travels within the set period is equal to the product of the current speed and the set period. Therefore, given the vehicle's pose, steering angle, and travel distance, the vehicle's next pose can be calculated.

[0052] In this embodiment, the period is set to 0.2 seconds.

[0053] S3-2-5-3: When i The time ends when the time equals 10; otherwise, let i Increment the value by 1, and repeat the trajectory deduction from step S3-2-5-1 to S3-2-5-3.

[0054] wherein, i =1, 2, 3... n , n represents a first set value, which is 10 in the embodiment.

[0055] When i =1, the vehicle pose of this trajectory deduction is the current vehicle pose; when i is greater than 1, the vehicle pose of this trajectory deduction is the first i -1 first iteration vehicle pose.

[0056] Finally, the first first iteration steering angle, the second first iteration steering angle,..., and the tenth first iteration steering angle, a total of 10 first iteration steering angles.

[0057] The first first iteration steering angle, the second first iteration steering angle,..., and the tenth first iteration steering angle, a total of 10 first iteration steering angles.

[0058] S3-2-6: Calculate a first iteration fitness according to a plurality of the first iteration vehicle poses, a reference trajectory, and the fitness function. In the process of calculating the first iteration fitness, a plurality of minimum distances from the plurality of second iteration vehicle poses to the reference trajectory need to be calculated first, and then the first iteration fitness is calculated by substituting the fitness function.

[0059] The specific process includes: S3-2-6-1: Calculate a first distance between the current vehicle pose and all the preset position points. Specifically, since the reference trajectory includes a plurality of preset position points, and only the specific parameter value of the position is known at the beginning, it is difficult to intuitively know which part is closer, so only the distances between the current vehicle pose and all the preset position points can be calculated.

[0060] Wherein, when calculating the distance between the vehicle pose and the preset position point, only the distance between the vehicle pose and the preset position point is calculated according to the position of the vehicle.

[0061] S3-2-6-2: Set a search radius according to the current speed, a set period, and the smallest first distance. It should be noted that: given that the vehicle travels a distance of the product of the current speed and the set period every set period. At the same time, the vehicle pose obtained after trajectory deduction will be closer to the reference trajectory than the current vehicle pose; that is, the minimum distance between the vehicle pose after trajectory deduction and the reference trajectory is less than the smallest first distance.

[0062] In this paper, the preset position point on the reference trajectory closest to the vehicle pose after trajectory deduction is referred to as the closest deduction point.

[0063] For the above reasons, the distance between the current vehicle pose and the deduced nearest point is less than the sum of the minimum first distance and the travel distance.

[0064] Specifically, the trajectory deduces 10 times in this embodiment, and the distance between the tenth deduced nearest point and the current vehicle pose is less than the sum of the minimum first distance and the total travel distance in 10 set periods. Therefore, the search radius can be set to ten times the product of the current speed and the set period, plus the minimum first distance.

[0065] In this way, the preset location points not selected as reference preset location points can be excluded, reducing the total amount of calculation and improving the calculation efficiency.

[0066] S3-2-6-3: The preset location points in the circle with the current vehicle pose as the center and the search radius as the radius are taken as reference preset location points; Since the first distance from the current vehicle pose to all preset location points has been calculated in step S3-2-6-1, the preset location points corresponding to the first distance less than the search radius can be selected as reference preset location points.

[0067] S3-2-6-4: Calculate the distances between all reference preset location points and all second iteration vehicle poses to obtain a plurality of second distances; According to the coordinates of the reference preset location points in the map and the coordinates of the second iteration vehicle poses in the map, the second distances are obtained by calculating the coordinate difference using the Pythagorean theorem formula.

[0068] S3-2-6-5: The minimum second distances corresponding to the plurality of second iteration vehicle poses are respectively taken as a plurality of minimum distances. For example, the minimum distance between the first iteration vehicle pose and the reference trajectory is denoted by d1. i R

[0069] S3-2-6-6: All minimum distances are substituted into the fitness function to calculate the first iteration fitness.

[0070] After step S3-2-6, the first iteration fitness can be calculated, and it can be judged whether the iteration prediction time domain parameter is closer to the ideal value.

[0071] ​​​S3-2-7: When the end condition of the sparrow algorithm is reached, the iteration prediction time domain parameter corresponding to the historical maximum value in the plurality of first iteration fitness is taken as the optimization prediction time domain parameter; otherwise, the positions of all sparrows in the first search range and the first iteration parameter are updated, and steps S3-2-3 to S3-2-7 are repeated.

[0072] In some embodiments, in order for the final scheme searched by the sparrow algorithm to match the actual situation, a constraint condition needs to be added to the sparrow algorithm.

[0073] In this embodiment, the constraint conditions include: Constraint condition 1: the steering angle of the front wheel of the vehicle is greater than -35° and less than 35° (right turn is positive, and the front wheel returns to the normal state of 0°) Constraint condition 2: the absolute value of the difference between the steering angles of the two adjacent set periods is less than or equal to 5°.

[0074] Constraint condition 3: the minimum first distance is less than 2 meters.

[0075] In this embodiment, the end condition is to reach the maximum number of iterations, that is, to end after 100 generations.

[0076] S3-3: Substitute the optimization prediction time domain parameter into the initial model predictive control system to obtain the model predictive control system.

[0077] S4: Based on the current vehicle speed, the current vehicle pose, the reference trajectory, the model predictive control system, and the fitness function, the state weight matrix and the output weight matrix are optimized by using the sparrow algorithm (the second sparrow algorithm) to obtain the optimal state weight matrix and the optimal output weight matrix, including: S4-1: Set the second search range of the state weight matrix and the output weight matrix, and the second iteration parameter of the sparrow algorithm; Specifically, the state weight matrix and the output weight matrix are both diagonal matrices. The state weight matrix includes a first element, and the output weight matrix includes a second element.

[0078] The second search range is set as: the first element ∈ [10, 100]; and the second element ∈ [0.1, 10]. The initial value of the first element is 50, and the initial value of the second element is 1.

[0079] The second iteration parameter includes the number of sparrow populations, the initial position of the sparrow, the identity of the sparrow, and the maximum number of iterations (set to 100 generations in this embodiment); and also has the above constraint conditions; Updating the second iteration parameter includes: Updating the position of the sparrow according to the current identity of the sparrow; According to the second iteration fitness corresponding to the position of the sparrow of the last iteration, the identity of the sparrow is re-allocated. The specific allocation method is consistent with the foregoing.

[0080] In some embodiments, the proportion of the number of sparrow discoverers to the total number of sparrow decreases monotonically with the increase of the number of iterations. The specific method is consistent with the foregoing.

[0081] S4-2: According to the second search range and the second iteration parameter, the iteration state weight matrix and the iteration output weight matrix corresponding to all sparrows in the sparrow algorithm are calculated; Specifically, according to the identity of the current sparrow (belongs to the second iteration parameter), the combination of the state weight matrix and the output weight matrix in the second search range after iteration is calculated by selecting the corresponding position update formula. Since the position of the sparrow is actually a specific value in the second search range, the iteration state weight matrix and the iteration output weight matrix can be directly obtained here.

[0082] S4-3: Based on the model predictive control system, according to the current vehicle speed, the current vehicle pose, the reference trajectory, the iteration state weight matrix and the iteration output weight matrix, a plurality of second iteration steering angles and a plurality of second iteration vehicle poses are calculated; Step S4-3 is consistent with the process of the foregoing trajectory deduction, and the difference is only that the data is different. Specifically, it includes: S4-3-1: input the current vehicle speed, the current vehicle pose, the reference trajectory, the iteration state weight matrix and the iteration output weight matrix into the model predictive control system to obtain one second iteration steering angle; S4-3-2: update the second iteration vehicle pose according to the current second iteration vehicle pose, the current vehicle speed and the latest obtained second iteration steering angle; the current second iteration vehicle pose is initially the current vehicle pose, and subsequently is the second iteration vehicle pose obtained by the last update; S4-3-3: repeat steps S4-3-1 to S4-3-3 until the total number of second iteration vehicle poses reaches a first set value.

[0083] According to the steps of S4-3, a plurality of second iteration vehicle poses can be calculated in order to calculate the fitness.

[0084] S4-4: According to the plurality of second iteration vehicle poses, the reference trajectory and the fitness function, a second iteration fitness is calculated; the specific principle is consistent with step S3-2-6 in the foregoing, which will not be repeated here; only the difference between the parameters for calculation exists.

[0085] S4-5: When the end condition of the sparrow algorithm is reached, the state weight matrix and the output weight matrix corresponding to the historical maximum value in the plurality of second iteration fitness are taken as the optimal state weight matrix and the optimal output weight matrix; otherwise, the positions of all sparrows in the second search range and the second iteration parameter are updated, and steps S4-2 to S4-5 are repeated.

[0086] After going through the above steps, the optimal state weight matrix and the optimal output weight matrix are obtained, and the updated model predictive control system can be automatically controlled to control the front wheel steering of the vehicle.

[0087] S5: inputting the current vehicle speed, the current vehicle pose, the reference trajectory, the optimal state weight matrix and the optimal output weight matrix into the model predictive control system, calculating the optimal steering angle, and automatically controlling the front wheel of the vehicle to adjust to the optimal steering angle; S6: every interval, determine whether to end; if yes, stop repeating steps S1 to S6; otherwise, continue to repeat steps S1 to S6.

[0088] In this embodiment, the interval is 0.2 seconds. Whether to end is determined according to whether an end instruction is received. When the end instruction is received, it is determined to end; otherwise, it is determined to not end, and steps S1 to S6 are further repeated.

[0089] In order to more accurately control the steering angle of the front wheel of the vehicle to track the reference trajectory, the interval is set to be short, so as to avoid a long control vacuum period and accumulate a large error.

[0090] The above description is only the preferred embodiment of the present application and the explanation of the applied technical principles. Those skilled in the art should understand that the scope of the present application is not limited to the technical solutions formed by the specific combination of the above technical features, and should also cover other technical solutions formed by any combination of the above technical features or equivalent features without departing from the inventive concept. For example, the above features are replaced with the technical features disclosed in the present application (but not limited to) having similar functions to form technical solutions.

Claims

1. A vehicle trajectory automatic tracking method based on a model predictive control system, characterized in that, The method comprises the following steps: S1: obtaining a current vehicle speed, a current vehicle pose and a reference trajectory; The reference trajectory is a preset driving trajectory of the vehicle; S2: constructing a fitness function according to the current vehicle pose and the reference trajectory; S3: updating a model predictive control system; the input of the model predictive control system comprises the current vehicle speed, the current vehicle pose, the reference trajectory, a state weight matrix and an output weight matrix, and the output is a steering angle; the model predictive control system is further configured to automatically control the vehicle to adjust a front wheel according to the steering angle; S4: based on the current vehicle speed, the current vehicle pose, the reference trajectory, the model predictive control system and the fitness function, performing optimization on the state weight matrix and the output weight matrix by using a sparrow algorithm to obtain an optimal state weight matrix and an optimal output weight matrix; S5: inputting the current vehicle speed, the current vehicle pose, the reference trajectory, the optimal state weight matrix and the optimal output weight matrix into the model predictive control system to calculate an optimal steering angle, and automatically controlling the front wheel of the vehicle to adjust to the optimal steering angle; S6: repeating steps S1 to S6 every interval of a set period.

2. The vehicle trajectory automatic tracking method based on model predictive control system according to claim 1, characterized in that, The updated model predictive control system comprises: S3-1: obtaining an initial model predictive control system; the initial model predictive control system has a prediction time domain parameter; the initial model predictive control system calculates a steering angle according to the prediction time domain parameter, the current vehicle speed, the current vehicle pose, the reference trajectory, a state weight matrix and an output weight matrix; S3-2: based on the current vehicle speed, the current vehicle pose, the reference trajectory, the initial model predictive control system and the fitness function, performing optimization on the prediction time domain parameter by using a sparrow algorithm to obtain an optimized prediction time domain parameter; S3-3: substituting the optimized prediction time domain parameter into the initial model predictive control system to obtain the model predictive control system.

3. The vehicle trajectory automatic tracking method based on model predictive control system according to claim 2, characterized in that, The method of performing optimization on the prediction time domain parameter by using a sparrow algorithm based on the current vehicle speed, the current vehicle pose, the reference trajectory, the initial model predictive control system and the fitness function to obtain an optimized prediction time domain parameter comprises: S3-2-1: obtaining an initial state weight matrix and an initial output weight matrix of a model predictive control system currently used by the vehicle; S3-2-2: setting a first search range of the prediction time domain parameter and a first iteration parameter of the sparrow algorithm; S3-2-3: obtaining iteration prediction time domain parameters corresponding to all sparrows in the sparrow algorithm according to the first search range and the first iteration parameter; S3-2-4: substituting the iteration prediction time domain parameters into the initial model predictive control system to obtain iteration model predictive control systems; S3-2-5: based on the iteration model predictive control systems, calculating a plurality of first iteration steering angles and a plurality of first iteration vehicle poses according to the current vehicle speed, the current vehicle pose, the reference trajectory, the initial state weight matrix and the initial output weight matrix; S3-2-6: calculating a first iteration fitness according to a plurality of the first iteration vehicle poses, the reference trajectory and the fitness function. S3-2-7: When the end condition of the sparrow algorithm is reached, the iteration prediction time domain parameter corresponding to the historical maximum value in the plurality of first iteration fitness is taken as the optimized prediction time domain parameter; otherwise, the positions of all sparrows in the first search range and the first iteration parameter are updated, and steps S3-2-3 to S3-2-7 are repeated.

4. The vehicle trajectory automatic tracking method based on model predictive control system according to claim 1, characterized in that, The sparrow algorithm is used for optimization of the state weight matrix and the output weight matrix based on the current vehicle speed, the current vehicle pose, the reference trajectory, the model predictive control system and the fitness function, to obtain the optimal state weight matrix and the optimal output weight matrix, which comprises: S4-1: setting a second search range of the state weight matrix and the output weight matrix, and a second iteration parameter of the sparrow algorithm; S4-2: calculating the iteration state weight matrix and the iteration output weight matrix corresponding to all sparrows in the sparrow algorithm according to the second search range and the second iteration parameter; S4-3: based on the model predictive control system, calculating a plurality of second iteration steering angles and a plurality of second iteration vehicle poses according to the current vehicle speed, the current vehicle pose, the reference trajectory, the iteration state weight matrix and the iteration output weight matrix; S4-4: calculating a second iteration fitness according to a plurality of second iteration vehicle poses, a reference trajectory and the fitness function; S4-5: when the end condition of the sparrow algorithm is reached, the state weight matrix and the output weight matrix corresponding to the historical maximum value in the plurality of second iteration fitness are taken as the optimal state weight matrix and the optimal output weight matrix; otherwise, the positions of all sparrows in the second search range and the second iteration parameter are updated, and steps S4-2 to S4-5 are repeated.

5. The vehicle trajectory automatic tracking method based on model predictive control system according to claim 4, characterized in that, The second iteration parameter comprises: initial positions of sparrows, identities of sparrows; the identity of the sparrow comprises: a finder, a follower and a guard; The updating of the second iteration parameter comprises: updating the position of the sparrow according to the current identity of the sparrow; redistributing the identity of the sparrow according to the second iteration fitness corresponding to the position of the sparrow in the last iteration.

6. The vehicle trajectory automatic tracking method based on model predictive control system according to claim 5, wherein, The proportion of the number of finder sparrows to the total number of sparrows monotonically decreases with the increase of the number of iterations.

7. The vehicle trajectory automatic tracking method based on model predictive control system according to claim 4, characterized in that, The sparrow algorithm is used for optimization of the state weight matrix and the output weight matrix based on the current vehicle speed, the current vehicle pose, the reference trajectory, the model predictive control system and the fitness function, to obtain the optimal state weight matrix and the optimal output weight matrix, which comprises: S4-3-1: inputting the current vehicle speed, the current vehicle pose, the reference trajectory, the iteration state weight matrix and the iteration output weight matrix into the model predictive control system to obtain one second iteration steering angle; S4-3-2: updating the second iteration vehicle pose according to the current second iteration vehicle pose, the current vehicle speed and the latest obtained second iteration steering angle; the current second iteration vehicle pose is initially the current vehicle pose, and subsequently is the second iteration vehicle pose obtained after the last update; S4-3-3: repeating steps S4-3-1 to S4-3-3 until the total number of second iteration vehicle poses reaches a first set value.

8. The vehicle trajectory automatic tracking method based on model predictive control system according to claim 7, characterized in that, The second iteration fitness is calculated according to the plurality of second iteration vehicle poses, the reference trajectory and the fitness function, and the second iteration fitness comprises: a plurality of minimum distances from the plurality of second iteration vehicle poses to the reference trajectory are calculated; all the minimum distances are substituted into the fitness function to calculate the second iteration fitness.

9. The vehicle trajectory automatic tracking method based on model predictive control system according to claim 8, wherein, The reference trajectory comprises a plurality of preset position points. The plurality of minimum distances from the plurality of second iteration vehicle poses to the reference trajectory are calculated, and the calculation comprises: a first distance between a current vehicle pose and all the preset position points is calculated; a search radius is set according to a current vehicle speed, a set period and the smallest first distance; the preset position points in a circle with the current vehicle pose as a center and the search radius as a radius are taken as reference preset position points; a plurality of second distances between all the reference preset position points and all the second iteration vehicle poses are calculated; the minimum second distances corresponding to the plurality of second iteration vehicle poses are respectively taken as the plurality of minimum distances.

Images