Automatic parking trajectory planning method and system based on adaptive collision avoidance constraints

Abstract

The present invention belongs to the technical field of automatic driving, and specifically relates to an automatic parking trajectory planning method and system based on adaptive collision avoidance constraints, comprising: finding a series of intermediate nodes based on a coarse path generated by A*, and then using hybrid A* to connect the intermediate nodes with a starting point and an end point to obtain an initial feasible trajectory; in the second stage, a cost function is set considering parking time and comfort, and an optimal control problem is established in combination with adaptive constraints, and the trajectory obtained in the first stage is used as an initial solution to the optimization problem, thereby solving the optimal trajectory; the present invention proposes a new method for finding intermediate nodes based on the orientation angle of the initial path point, and divides the global trajectory planning into several relatively simple planning tasks. The method can effectively improve the efficiency of hybrid A* in finding feasible trajectories in a global map.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous driving, and in particular relates to an automatic parking trajectory planning method and system based on adaptive collision avoidance constraints. Background Art

[0002] At present, the further narrowing of parking spaces and the increasing complexity of the parking environment have made the problem of "difficult parking" more prominent. The development of autonomous driving technology provides a potential solution for parking. Trajectory planning is one of the three key technologies of autonomous driving. It plays an important role in linking upper-level environmental perception with lower-level tracking control, aiming to generate a safe, comfortable and optimal parking route. According to the data, the complexity of the parking environment and the narrowness of parking spaces have greatly increased the parking time and brought great challenges to the planning task. Therefore, it is crucial to propose a planning method that can quickly find a feasible parking trajectory in a narrow scene.

[0003] The prior art has the following problems:

[0004] 1. Planning method: Global trajectory planning methods are mainly divided into sampling-based, search-based and optimization-based. Sampling- and search-based methods plan trajectories by discretizing the continuous state space into a node graph. However, the trajectory planned by this method is of poor quality and difficult for autonomous vehicles to track. The optimization-based method optimizes through a series of constraints and cost functions to obtain an easy-to-track trajectory. However, the optimization method requires a good initial solution, otherwise the optimization time will be greatly extended or even fail. Therefore, the sampling- or search-based method is combined with the optimization-based method to improve the efficiency of trajectory planning and quickly obtain a comfortable and smooth trajectory.

[0005] 2. Establishing obstacle avoidance constraints: The current mainstream methods for establishing obstacle avoidance constraints are divided into establishing a cost function or expanding obstacles to establish a driving corridor. The method of establishing a cost function cannot strictly guarantee that there is no collision between the vehicle and the obstacle, and the parameters are difficult to adjust in different environments; the method of establishing a driving corridor by expanding obstacles will not generate a large amount of calculation as the number of obstacles increases, but the expansion will ignore the drivable areas of some vehicles, which may result in poor quality of the planned trajectory, and even inability to plan a feasible trajectory in some narrow channels. Therefore, there is an urgent need for a method that can quickly establish obstacle avoidance constraints in wide or narrow channel environments to ensure that collisions with obstacles are avoided. Summary of the invention

[0006] In order to solve the above technical problems, the present invention proposes an automatic parking trajectory planning method and system based on adaptive collision avoidance constraints.

[0007] The present invention achieves the above-mentioned purpose through the following technical solutions:

[0008] In a first aspect, the present invention proposes an automatic parking trajectory planning method based on adaptive collision avoidance constraints, the method comprising:

[0009] S1. Obtain the environmental information of the target vehicle to generate a grid map, and generate a two-dimensional feasible path on the grid map through the A* algorithm and a preset node generation algorithm, as well as a set including a starting point, an intermediate node, and an end point;

[0010] S2. Use the hybrid A* algorithm to connect all the points in the set. During the hybrid A* connection process, if it is detected that the time taken to connect a certain adjacent intermediate node is too long, or the parking operation mode is repeatedly changed, then delete the intermediate node, and traverse the set to generate a final feasible trajectory;

[0011] S3. Determine the adaptive collision constraints of the target vehicle based on the vehicle kinematic constraints and surrounding environmental factors, set the cost function according to the parking time and comfort of the target vehicle, establish the optimal control strategy in combination with the adaptive collision constraints, and solve the final feasible trajectory to determine the optimal trajectory.

[0012] As a further optimization solution of the present invention, in step S1, a two-dimensional feasible path x is generated on the grid map by using the A* algorithm. Astar (x s ,y s ), calculate the heading angle of each path point and record it as set S θ , the starting point angle is θ i ; The preset node generation algorithm is specifically:

[0013] Starting node to χ Astar The last node in the i Subtract the current node angle θ c And determine whether the difference is less than the set value. If so, the counter b is increased by 1 and the current node is placed in the set S d If not, check whether the counter b is greater than τ thre If so, change S d The last node of S is placed m If not, reset the counter to zero and set θ c The value of θ is assigned to i , S m The starting point, intermediate nodes and end point are stored in it.

[0014] As a further optimization solution of the present invention, in step S2, if it is detected that the time taken by the hybrid A* to connect adjacent intermediate nodes exceeds T0 or the vehicle operation frequently switches between forward and backward, then the intermediate node as the end point will be deleted, S mThe next node will become the new endpoint of hybrid A*.

[0015] As a further optimization solution of the present invention, generating a final feasible trajectory after traversing the set includes:

[0016] After all the hybrid A* algorithm search processes are completed, a rough trajectory λ1 is generated, and the final feasible trajectory λ0 obtains all other decision variables based on the rough trajectory λ1. get, Determined based on trajectory length, vehicle maximum speed and acceleration, v 0 , a 0 ,φ 0 ,ω 0 As shown below:

[0017]

[0018]

[0019]

[0020]

[0021] As a further optimization scheme of the present invention, in step S3, the adaptive collision constraint includes using two discs to cover the rectangular outline of the target vehicle in the wide channel, abstracting the target vehicle as the center point of the two discs, and expanding the obstacle based on the radius of the two discs, and expanding the rectangle in four directions along each point on the final feasible trajectory λ0, thereby obtaining a drivable area;

[0022] The obstacle avoidance constraint is expressed as:

[0023]

[0024]

[0025] Among them, (x i ,y i ) is the center coordinate of the i-th disk, the continuous trajectory x(t) is discretized into a series of (n+1) trajectory points, namely [x(0), x(1), ..., x(n)], tf represents the parking time; x i =(x f (t), x r (t)), y i =(y f (t), y r (t)), where (x f ,y f ) and (x r ,yr ) represent the coordinates of the centers of the two disks at the front and rear of the vehicle, respectively. The geometric constraints can be obtained according to the geometric dimensions of the vehicle:

[0026]

[0027]

[0028]

[0029]

[0030] The distance from the center of each disk to the nearest obstacle must be greater than or at least equal to R d , R d The calculation formula is:

[0031]

[0032] As a further optimization solution of the present invention, in step S3, the adaptive collision constraint further includes: in a narrow channel, the standard for detecting the vehicle driving environment is as follows:

[0033]

[0034]

[0035] Get the coordinates of the four corner points of the target vehicle's center position and connect them into a rectangle to perform collision detection with the surrounding polygonal obstacles. If there is no intersection between the two polygons, it means that there is no collision between the vehicle and the obstacle. Expand in four directions through each discrete point of the rectangle to obtain the maximum drivable area. The coordinates of the four vertices of the vehicle are calculated as follows:

[0036]

[0037] The adaptive collision avoidance constraint is expressed as follows:

[0038]

[0039]

[0040] Where N s To N s+i Indicates that the trajectory point is in a narrow channel.

[0041] As a further optimization solution of the present invention, in step S3, setting the cost function according to the parking time and comfort of the target vehicle includes:

[0042] The cost function is defined as follows:

[0043]

[0044] Among them, μ1>0, μ2>0 and μ3>0 represent weighting parameters, t f Penalize the parking time, acceleration ||v(k+1)-v(k)|| 2 Angular velocity||θ(k+1)-θ(k)|| 2 Penalize the smoothness and comfort of the parking trajectory.

[0045] As a further optimization scheme of the present invention, in step S3, the step of establishing the optimal control strategy in combination with the adaptive constraint and solving the final feasible trajectory to determine the optimal trajectory includes:

[0046] A while loop is introduced to construct an intermediate optimal control problem to iteratively optimize the final feasible trajectory. The nonlinear vehicle kinematic constraints and geometric constraints are softened as external penalty costs and put into the cost function so that the nonlinear kinematic constraints and geometric constraints meet the requirements, and then the iteration is stopped, as shown in the following formula:

[0047] min J+ω p ·(J (1) +J (2) )

[0048] Among them, ω p represents the weight parameter, J (1) and J (2) Represent the corresponding penalties for kinematic constraints and geometric constraints respectively;

[0049] In each loop iteration, the driving corridor is regenerated based on the optimal trajectory obtained last time, and the optimal trajectory in the current corridor is solved by the primal-dual interior point solver.

[0050] In a second aspect, the present invention proposes an automatic parking trajectory planning system based on adaptive collision avoidance constraints, the system comprising:

[0051] A coarse path generation module is used to obtain the environmental information of the target vehicle to generate a grid map, and generate a two-dimensional feasible path on the grid map through the A* algorithm and a preset node generation algorithm, as well as a set of starting points, intermediate nodes, and end points;

[0052] A singular point removal module is used to connect all points in the set using a hybrid A* algorithm. During the hybrid A* connection process, if it is detected that the time taken to connect a certain adjacent intermediate node is too long, or the parking operation mode is repeatedly changed, the intermediate node is deleted, and the final feasible trajectory is generated after traversing the set;

[0053] The trajectory determination module is used to determine the adaptive collision constraints of the target vehicle based on the vehicle kinematic constraints and surrounding environmental factors, set the cost function according to the parking time and comfort of the target vehicle, establish the optimal control strategy in combination with the adaptive collision constraints, and solve the final feasible trajectory to determine the optimal trajectory.

[0054] The beneficial effects of the present invention are:

[0055] 1. This paper proposes a new method for finding intermediate nodes based on the orientation angle of the initial path point, which divides the global trajectory planning into several simpler planning tasks. This method can effectively improve the efficiency of hybrid A* in finding feasible trajectories in the global map.

[0056] 2. When the present invention uses the hybrid A* and intermediate node method to find a feasible trajectory, the existence of some singular intermediate nodes will greatly increase the planning time. The present invention designs a singular node removal strategy. When it is detected that the hybrid A* takes too long at two adjacent intermediate nodes, or the vehicle repeatedly moves forward and backward, the corresponding intermediate node is deleted, and the trajectory planning is restarted with the next node as the intermediate node, thereby realizing the use of hybrid A* to quickly find a feasible trajectory.

[0057] 3. The present invention proposes a method for establishing an adaptive obstacle avoidance constraint. The method adaptively establishes the obstacle avoidance constraint by detecting the environment in which the autonomous driving vehicle is located, so that the vehicle can obtain sufficient drivable area regardless of whether it is in a wide channel or a narrow channel, thereby speeding up the solution of the optimization problem and improving the quality of trajectory optimization. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1 A schematic diagram of a flow chart of the trajectory planning method in the present invention.

[0059] Figure 2 It is the overall framework diagram of the trajectory planning method in the present invention.

[0060] Figure 3 It is a pseudo code diagram of the while loop in the trajectory planning method according to a specific embodiment of the present invention.

[0061] Figure 4 It is a schematic diagram of searching trajectories by enhanced hybrid A* in the trajectory planning method in a specific embodiment of the present invention.

[0062] Figure 5 It is a schematic diagram of the whole vehicle motion model of the target vehicle in the trajectory planning method according to the specific implementation mode of the present invention.

[0063] Figure 6 This is a schematic diagram of parking simulation results in a parking lot according to Example 1 of a specific implementation manner of the present invention.

[0064] Figure 7-10 This is a schematic diagram of the speed, angular velocity, acceleration and steering angle of the optimized trajectory during parking in a parking lot in Example 1 of the specific implementation example of the present invention.

[0065] Fig.11 This is a diagram of the QCar experimental architecture of Case 2 of the specific implementation method of the present invention.

[0066] Fig.12 This is a schematic diagram of the experimental trajectory results of the QCar platform in Case 2 of the specific implementation method of the present invention.

[0067] Fig.13 Schematic diagram of longitudinal and lateral errors of the QCar platform experiment in Case 2 of the specific implementation mode of the present invention. DETAILED DESCRIPTION

[0068] For ease of understanding, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0069] Example 1

[0070] The mainstream obstacle avoidance constraint establishment methods based on the existing technology are divided into establishing a cost function, or expanding obstacles to establish a driving corridor. The method of establishing a cost function cannot strictly guarantee that there is no collision between the vehicle and the obstacle, and the parameters are difficult to adjust in different environments; the method of establishing a driving corridor by expanding obstacles will not generate a large amount of calculation as the number of obstacles increases, but the expansion will ignore the drivable areas of some vehicles, which may result in poor quality of the planned trajectory, or even inability to plan a feasible trajectory in some narrow channels. Therefore, there is an urgent need for a method that can quickly establish obstacle avoidance constraints in wide or narrow channel environments to ensure that collisions with obstacles are avoided. Figure 1-5 As shown, this embodiment proposes an automatic parking trajectory planning method based on adaptive collision avoidance constraints, and the method includes:

[0071] S1. Obtain the environmental information of the target vehicle to generate a grid map, and generate a two-dimensional feasible path on the grid map through the A* algorithm and a preset node generation algorithm, as well as a set including a starting point, an intermediate node, and an end point;

[0072] S2. Use the hybrid A* algorithm to connect all the points in the set. During the hybrid A* connection process, if it is detected that the time taken to connect a certain adjacent intermediate node is too long, or the parking operation mode is repeatedly changed, then delete the intermediate node, and traverse the set to generate a final feasible trajectory;

[0073] S3. Determine the adaptive collision constraints of the target vehicle based on the vehicle kinematic constraints and surrounding environmental factors, set the cost function according to the parking time and comfort of the target vehicle, establish the optimal control strategy in combination with the adaptive collision constraints, and solve the final feasible trajectory to determine the optimal trajectory.

[0074] As an embodiment of the present invention, the above trajectory planning method proposes a two-stage enhanced hybrid A* parking trajectory planning method. In the first stage, a series of intermediate nodes are first found based on the rough path (two-dimensional feasible path) generated by A*, and then the intermediate nodes are connected with the starting point and the end point using hybrid A* to obtain the initial feasible trajectory; in the second stage, the cost function is set considering the parking time and comfort, and the optimal control problem is established in combination with the adaptive constraints. The trajectory obtained in the first stage is used as the initial solution to the optimization problem, thereby solving the optimal trajectory. The overall structural block diagram of the present invention is shown in FIG. Figure 2 As shown, its pseudo code is as follows Figure 3 As shown, the detailed steps of the present invention are described below.

[0075] 1. Enhanced hybrid A* trajectory planning method based on intermediate nodes:

[0076] When ordinary hybrid A* performs global planning, when the global map is large and the planning task is complex, the planning time will increase significantly and the planning success rate will be greatly reduced. When the intermediate nodes are introduced, the complex planning problem is divided into several simpler trajectory planning problems, which can greatly shorten the planning time and improve the planning success rate. The schematic diagram of the proposed enhanced hybrid A* method for finding feasible paths is shown in Figure 4 shown.

[0077] The enhanced hybrid A* can be divided into the following steps:

[0078] (1) Obstacle expansion is performed based on environmental information to obtain a grid map, such as Figure 4 (a)

[0079] (2) Based on the grid map, the traditional A* algorithm is used to quickly generate a two-dimensional feasible path χ Astar (x s ,y s ),like Figure 4 (b)

[0080] (3) After obtaining the two-dimensional path χ Astar After that, the heading angle of each path point is calculated and saved in the set S θ Then the starting point angle is recorded as θ i From the starting node to χ Astar The last node in the i Subtract the current point angle θc And determine whether the difference is less than a certain value. If so, the counter b is increased by 1 and the current node is placed in the set S d If not, we check if the counter value is greater than τ thre If yes, change S d The last node of S is placed m If not, the above instruction will not be executed, and then the counter will be reset to zero and θ c The value of θ is assigned to i This method takes less time to find the intermediate nodes and avoids taking some points with fast heading angle changes as the intermediate nodes, which significantly reduces the number of intermediate nodes. Astar The end point, χ Astar(Ne) Will be placed in S m In the collection. m The starting point, end point, and intermediate nodes are saved in the next step. The hybrid A* is used to connect these nodes to obtain a feasible trajectory.

[0081] 2. Strange node deletion strategy:

[0082] After obtaining the set of intermediate points S m After that, these points Γ node will be set as the new starting and ending points. A hybrid A* algorithm is used to quickly search between each intermediate point (such as Figure 4 (d)). However, there may be some singular intermediate nodes whose heading angles are not properly selected. In narrow passages, the vehicle needs to repeatedly perform forward and backward operations to reach the specified heading angle. These singular nodes will not only lead to low efficiency in finding trajectories by the hybrid A* algorithm, but also increase energy consumption, increase parking time, and other issues. Therefore, the present invention designs a singular node deletion strategy to detect the time used by the hybrid A* to connect adjacent intermediate nodes and the parking operation mode. If the time used exceeds T0 or the vehicle's operation frequently switches between forward and backward, then the intermediate node as the end point will be deleted, S m The next node will become the new end point of the hybrid A*. After all the hybrid A* algorithm search processes are completed, the trajectory λ1 is generated. Figure 2 In line 29, λ0←generates the initial trajectory (λ1), which mainly obtains all other decision variables based on the rough trajectory λ1. After traversing the set, the final feasible trajectory is generated, including:

[0083] After all the hybrid A* algorithm search processes are completed, the rough trajectory λ1 is generated, and the final feasible trajectory λ0 obtains all other decision variables based on the rough trajectory λ1. get, Determined based on trajectory length, vehicle maximum speed and acceleration, v 0, a 0 ,φ 0 ,ω 0 As shown below:

[0084]

[0085]

[0086]

[0087]

[0088] At this point, we have obtained the feasible trajectory λ0, which is used as the initial solution for the second stage optimization.

[0089] 3. Optimization method based on adaptive constraints:

[0090] In this stage, the initial guess λ0 above is used to warm-start the numerical optimization process. The optimization problem is established including a suitable objective function and some constraints.

[0091] (1) Vehicle kinematic constraints:

[0092] This embodiment uses a classic bicycle model to describe the kinematic equality constraints of the vehicle.

[0093]

[0094] Where x(t), y(t) represent the coordinates of the center point of the rear axle of the vehicle. f represents the time it takes for the vehicle to reach the destination, v is the longitudinal velocity, a is the acceleration, ω is the front wheel steering angular velocity, and L is the wheelbase. The vehicle state x(t) can be expressed as [x(t), y(t), θ(t), v(t), φ(t)]′, and the control state U(t) is defined as [a(t), ω(t)]′. In addition to the above parameters, some geometric parameters of the vehicle are also as follows Figure 5 As shown. f and L r Respectively represent the distance between the front bumper and the front axle, and the distance between the rear bumper and the rear axle of the vehicle. W is the width of the vehicle. Due to the physical or mechanical limitations related to vehicle kinematics, the aforementioned states and control variables have certain constraint ranges, namely:

[0095] |a(t)|≤a max ,|v(t)|≤v max

[0096] |w(t)|≤Ω max ,|φ(t)|≤φ max , t∈[0,t f ]

[0097] (2) Starting point and end point constraints: The starting point and end point constraints specify the initial and end state variables of parking.

[0098]

[0099] where x start ,y start ,θ start 、x f ,y f and θ f are the horizontal and vertical coordinates and the orientation angle of the vehicle's starting point and end point, respectively. In the present invention, it is assumed that the vehicle is stationary at the starting point and end point of the parking task.

[0100] (3) Adaptive collision constraints:

[0101] In step S3, the adaptive collision constraint includes: constructing a driving corridor method, specifically determining that the target vehicle is in a wide channel based on vehicle kinematic constraints and start and end point constraints, using two discs to cover the rectangular outline of the target vehicle, abstracting the target vehicle as the center point of the two discs, and expanding the obstacle based on the radius of the two discs, and expanding the rectangle in four directions of up, down, left and right along each point on the final feasible trajectory λ0, so as to obtain a drivable area;

[0102] The obstacle avoidance constraint is expressed as:

[0103]

[0104]

[0105] Among them, (x i ,y i ) is the center coordinate of the i-th disk, and the continuous trajectory x(t) is discretized into a series of (n+1) trajectory points, namely [x(0), x(1), ..., x(n)], t f Indicates parking time; Among them, (x f ,y f ) and (x r ,y r ) represent the coordinates of the centers of the two disks at the front and rear of the vehicle, respectively. The geometric constraints can be obtained according to the geometric dimensions of the vehicle:

[0106]

[0107]

[0108]

[0109]

[0110] The distance from the center of each disk to the nearest obstacle must be greater than or at least equal to R d , R d The calculation formula is:

[0111]

[0112] It should be noted that in some relatively narrow passages, expanding the obstacle map may result in a very narrow drivable corridor, which will affect the quality of the solution to the optimal control problem or even make it impossible to find a feasible solution. In addition, using a disk to cover the vehicle body and expanding the obstacle map with the disk radius for approximation will ignore some potential drivable areas. This approximation method has errors and has some limitations in use in relatively narrow environments. To solve the above problems, we designed an adaptive collision avoidance constraint to replace the previous collision avoidance constraint.

[0113] Preferably, in step S3, the above method is used to quickly obtain the driving corridor of the vehicle, as shown in the above obstacle avoidance constraints, and then the environment of the vehicle is detected based on the driving corridor.

[0114] Adaptive collision constraints also include: determining that the target vehicle is in a narrow channel based on vehicle kinematic constraints and start and end point constraints. The criteria for detecting the vehicle driving environment are as follows:

[0115]

[0116]

[0117] Get the coordinates of the four corner points of the target vehicle's center position and connect them into a rectangle to perform collision detection with the surrounding polygonal obstacles. If there is no intersection between the two polygons, it means that there is no collision between the vehicle and the obstacle. Expand in four directions through each discrete point of the rectangle to obtain the maximum drivable area. The coordinates of the four vertices of the vehicle are calculated as follows:

[0118]

[0119] Combining the above two obstacle avoidance constraint establishment methods, we finally get an adaptive collision avoidance constraint suitable for wide channels or narrow channels. The adaptive collision avoidance constraint is expressed as follows:

[0120]

[0121]

[0122] Where N s To N s+i Indicates that the trajectory point is in a narrow channel.

[0123] Preferably, in step S3, setting the cost function according to the parking time and comfort of the target vehicle includes:

[0124] The cost function is defined as follows:

[0125]

[0126] Among them, μ1>0, μ2>0 and μ3>0 represent weighting parameters, t f Penalize the parking time, acceleration ||v(k+1)-v(k)|| 2 Angular velocity||θ(k+1)-θ(k)|| 2 Penalize the smoothness and comfort of the parking trajectory.

[0127] Preferably, in step S3, the second stage aims to optimize the trajectory by constructing an iterative framework. Since the driving corridor is constructed based on the low-quality initial solution χ1 generated in the first stage, there may be a certain loss of drivable space. Therefore, one time based on the initial solution is not always effective. To solve this problem, we introduced a while loop for multiple iterative optimizations. The specific steps are: in each iteration, an intermediate optimal control problem (OCP) is constructed, and the OCP is established by obtaining a new adaptive collision avoidance constraint through the optimal solution of the previous iteration. Through this iterative framework, the drivable space can be maximized and the quality of the solution can be improved. Specifically, the intermediate OCP is put into the cost function as an external penalty cost by softening the nonlinear kinematic constraints and geometric constraints.

[0128] The optimal control strategy is established in combination with adaptive constraints and the optimal trajectory is determined by solving the final feasible trajectory, including:

[0129] The while loop is introduced to construct the intermediate optimal control problem to iteratively optimize the final feasible trajectory. By softening the nonlinear vehicle kinematic constraints and geometric constraints as external penalty costs and putting them into the cost function, the iteration is stopped when the nonlinear kinematic constraints and geometric constraints meet the requirements, as shown in the following formula:

[0130] min J+ω p ·(J (1) +J (2) )

[0131] Among them, ω p represents the weight parameter, J (1) and J (2) Represent the corresponding penalties for kinematic constraints and geometric constraints respectively;

[0132] Therefore, J (1) For example, it can be expressed as:

[0133]

[0134] Combination Figure 3 , Lines 30-45 describe the specific steps of the second stage. In line 31, some basic parameters are initialized, such as the current number of iterations κ iter , the maximum number of iterationsκ max , weighting parameter ω p And the kinematic infeasibility ρ inf Then, a solution is performed based on the feasible trajectory of the first stage to find the optimal trajectory and calculate the kinematic infeasibility of the trajectory, and then enter the while loop. In line 35, ρ tol represents the user-defined convergence threshold. In the while loop we check ρ inf <ρ tol Is it true? If so, it means that the nonlinear kinematic constraints and geometric constraints meet the requirements. Continue to check the current iteration number κ iter Is it greater than the maximum number of iterations κ? max , if yes, the solution fails. In each loop, we regenerate the driving corridor based on the optimal trajectory obtained last time, and solve the optimal trajectory under the current corridor through the primal-dual interior point solver IPOPT.

[0135] When the number of iterations reaches the maximum value or the kinematic infeasibility meets the requirements, we obtain a local optimal trajectory that is easy to track and meets the requirements of smoothness and comfort.

[0136] Based on the same inventive concept, the present invention proposes an automatic parking trajectory planning system based on adaptive collision avoidance constraints, the system comprising:

[0137] A coarse path generation module is used to obtain the environmental information of the target vehicle to generate a grid map, and generate a two-dimensional feasible path on the grid map through the A* algorithm and a preset node generation algorithm, as well as a set of starting points, intermediate nodes, and end points;

[0138] A singular point removal module is used to connect all points in the set using a hybrid A* algorithm. During the hybrid A* connection process, if it is detected that the time taken to connect a certain adjacent intermediate node is too long, or the parking operation mode is repeatedly changed, the intermediate node is deleted, and the final feasible trajectory is generated after traversing the set;

[0139] The trajectory determination module is used to determine the adaptive collision constraints of the target vehicle based on the vehicle kinematic constraints and surrounding environmental factors, set the cost function according to the parking time and comfort of the target vehicle, establish the optimal control strategy in combination with the adaptive collision constraints, and solve the final feasible trajectory to determine the optimal trajectory.

[0140] The trajectory planning method proposed in this application is exemplified below in combination with Case 1 and Case 2.

[0141] Case 1:

[0142] To evaluate the effectiveness of the proposed trajectory planning method, simulations and experiments were conducted in an indoor parking scenario. The simulations were performed in MATLAB / Simulink, and the experiments were conducted on the Quanser autonomous vehicle (QCar) platform.

[0143] In the present invention, a parking lot scene is designed, which is 40 meters long and wide. The drivable area is represented by white, while the brown with black cross area represents the space occupied by obstacles. The width of the passage is 5 meters. The vertical parking space is set to be 5.5 meters long and 2.5 meters wide. Figure 6 The key parameter settings of our trajectory planning method are shown in Table 1.

[0144] Table 1 Simulation parameter settings

[0145]

[0146] In this typical parking scenario, we can see that the car's trajectory is relatively smooth, without colliding with obstacles, and finally arrives at the parking space safely. Figure 7-10 It can be seen from the velocity v, angular velocity w, acceleration a and steering angle φ of the optimized trajectory that these values ​​meet the requirements of Table 1 and maintain a certain degree of smoothness.

[0147] In order to highlight the effectiveness and superiority of the new method proposed in the present invention, the proposed two-stage automatic parking trajectory planning framework is compared with existing planners: As shown in Table 2, the compared algorithms are general hybrid A*, an enhanced hybrid A* (EHA) and the method of this embodiment (IHA). All of the above algorithms divide trajectory planning into two steps: using A* or hybrid A* to find the initial trajectory, and then optimizing the initial trajectory with the IPOPT solver to obtain the final trajectory. In order to compare the performance of these algorithms, we denote the time spent on searching the initial path as T-IG, the time required to solve the numerical optimization process as T-OP, and t f Indicates the time required for the vehicle to reach the destination from the starting point. We also list in the table the efficiency improved by the method of this embodiment compared with the existing method.

[0148] Table 2 Comparison of simulation results

[0149]

[0150]

[0151] The HybridA* algorithm is a classic trajectory planning method for finding the global path. The EHA algorithm combines the A* and hybrid A* algorithms, and can significantly reduce the time for finding the initial path compared to the hybrid A* algorithm. And a good initial path helps the EHA algorithm to generate an optimized trajectory faster. In addition, in the process of finding the initial path, the method of this embodiment finds the initial path based on the heading angle of the A* path point, rather than simply visiting feasible nodes along the A* path, thereby further reducing the time required. As can be seen from Table 2, due to the large map, Hybrid A* did not find a feasible path, and EHA took a long time to find and optimize the initial path. The method IHA of this embodiment improved the performance by 52.37% in the total planning time.

[0152] Case 2:

[0153] To further verify the practicality of the proposed trajectory planning method, we conducted field experiments on Quanser's small autonomous driving car platform QCar. The movement of the car is achieved through the throttle and steering servo motor. We placed 6 cameras on the ceiling of a 4×4 meter indoor scene to reflect the position of the car in real time. The QCar experimental architecture is as follows: Fig.11 shown.

[0154] The trajectory planning module consists of a hybrid A* path planning algorithm and an optimization method to provide a feasible trajectory for the tracking control module. Subsequently, we use a proportional-integral (PI) controller for longitudinal tracking and a proportional (P) controller for lateral tracking. QCar feeds back the speed and position signals to the control module as input. Due to the limitations of experimental conditions, we set up the experimental site into a compact 3.5×3.5 meter parking lot. White and yellow lines divide the boundaries, parking spaces, and aisles. Although QCar is smaller in scale than a real car, its kinematic model is very close to the real-world car model at lower speeds. After adjusting several parameters of the trajectory planning algorithm, we applied the algorithm to the QCar platform and obtained the following Fig.12 Results shown. Fig.12 The experimental scenario setting is similar to that in our simulation. The blue line represents the planned path and the brown line represents the actual trajectory of QCar. QCar successfully reaches the parking space without collision. In addition, the close alignment between the planned trajectory and the actual trajectory also proves that the planned trajectory is feasible, smooth and trackable. Fig.13 The longitudinal and lateral errors in the test are detailed and are within acceptable ranges, although there are some minor vibrations, which may be attributed to the controller.

[0155] In summary, both simulation and experimental results demonstrate the effectiveness of our trajectory planning method.

[0156] According to an embodiment of the present invention, the above-mentioned method for establishing adaptive obstacle avoidance constraints adaptively establishes obstacle avoidance constraints by detecting the environment in which the autonomous driving vehicle is located, so that the vehicle can obtain sufficient drivable area regardless of whether it is in a wide channel or a narrow channel, thereby speeding up the solution of the optimization problem and improving the quality of trajectory optimization.

[0157] The above formulas are all dimensionless and numerical calculations. The formula is a formula for the most recent real situation obtained by collecting a large amount of data and performing software simulation. The preset parameters and thresholds in the formula are set by technicians in this field according to actual conditions.

[0158] It is obvious to those skilled in the art that the embodiments of the present invention are not limited to the details of the above exemplary embodiments, and that the embodiments of the present invention can be implemented in other specific forms without departing from the spirit or basic features of the embodiments of the present invention. Therefore, from any point of view, the embodiments should be regarded as exemplary and non-restrictive, and the scope of the embodiments of the present invention is limited by the attached claims rather than the above description, so it is intended to include all changes that fall within the meaning and scope of the equivalent elements of the claims in the embodiments of the present invention. Any figure mark in the claims should not be regarded as limiting the claims involved. In addition, it is obvious that the word "comprising" does not exclude other units or steps, and the singular does not exclude the plural. Multiple units, modules or devices stated in the system, device or terminal claims can also be implemented by the same unit, module or device through software or hardware. The words first, second, etc. are used to indicate names, and do not indicate any particular order.

[0159] Finally, it should be noted that the above implementation modes are only used to illustrate the technical solutions of the embodiments of the present invention and are not intended to limit them. Although the embodiments of the present invention have been described in detail with reference to the above preferred implementation modes, those skilled in the art should understand that the technical solutions of the embodiments of the present invention may be modified or replaced by equivalents without departing from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. An automatic parking trajectory planning method based on adaptive collision avoidance constraints, characterized in that: The method comprises: S1. Obtain the environmental information of the target vehicle to generate a grid map, and generate a two-dimensional feasible path on the grid map through the A* algorithm and a preset node generation algorithm, as well as a set including a starting point, an intermediate node, and an end point; In step S1, a two-dimensional feasible path x is generated on the grid map by using the A* algorithm. Astar (x s ,y s ), calculate the heading angle of each path point and record it as set S θ , the starting point angle is θ i ; The preset node generation algorithm is specifically: Starting node to χ Astar The last node in the i Subtract the current node angle θ c And determine whether the difference is less than the set value. If so, the counter b is increased by 1 and the current node is placed in the set S d If not, check whether the counter b is greater than τ thre If so, change S d The last node of S is placed m If not, reset the counter to zero and set θ c The value of θ is assigned to i ,S m The starting point, intermediate nodes and end point are stored in it; S2. All points in the set are connected using a hybrid A* algorithm. During the hybrid A* connection process, if it is detected that the time taken to connect a certain adjacent intermediate node is too long, or the parking operation mode is repeatedly changed, the intermediate node is deleted, and the final feasible trajectory is generated after traversing the set; In S2, if it is detected that the time taken by the hybrid A* to connect adjacent intermediate nodes exceeds T0 or the vehicle operation frequently switches between forward and backward, then the intermediate node as the end point will be deleted, S m The next node will become the new end point of hybrid A*; S3. Determine the adaptive collision constraints of the target vehicle based on the vehicle kinematic constraints and surrounding environmental factors, set the cost function according to the parking time and comfort of the target vehicle, establish the optimal control strategy in combination with the adaptive collision constraints, and solve the final feasible trajectory to determine the optimal trajectory.

2. The automatic parking trajectory planning method based on adaptive collision avoidance constraints according to claim 1, characterized in that: The step of generating a final feasible trajectory after traversing the set includes: After all the hybrid A* algorithm search processes are completed, a rough trajectory λ1 is generated, and the final feasible trajectory λ0 obtains all other decision variables based on the rough trajectory λ1. get, Determined based on trajectory length, maximum vehicle speed and acceleration, ν 0 ,a 0 ,φ 0 ,ω 0 As shown below: Where v is the longitudinal velocity, a is the acceleration, θ is the body orientation angle, φ is the body orientation angle change rate, ω is the front wheel steering angular velocity, L is the wheelbase, (x(k), y(k)) is the coordinate of the center point of the rear axle of the vehicle, t f Indicates the total parking time.

3. The automatic parking trajectory planning method based on adaptive collision avoidance constraints according to claim 2 is characterized in that: In step S3, the adaptive collision constraint includes using two discs to cover the rectangular outline of the target vehicle in the wide channel, abstracting the target vehicle as the center point of the two discs, and expanding the obstacle based on the radius of the two discs, and expanding the rectangle in four directions of up, down, left and right along each point on the final feasible trajectory λ0, so as to obtain a drivable area; The obstacle avoidance constraint is expressed as: Among them, (x i ,y i ) is the center coordinate of the i-th disk, and the continuous trajectory x(t) is discretized into a series of (n+1) trajectory points, namely [x(0), x(1), ..., x(n)], t f Indicates parking time; x i =(x f (t),x r (t)), y i =(y f (t),y r (t)), where (x f ,y f ) and (x r , yr) represent the coordinates of the centers of the two disks at the front and rear of the vehicle respectively. The geometric constraints can be obtained according to the geometric dimensions of the vehicle: Among them, L is the wheelbase, L f Indicates the distance between the front wheel and the front end of the vehicle, L r Indicates the distance between the rear wheels and the rear end of the vehicle; The distance from the center of each disk to the nearest obstacle must be greater than or at least equal to R d , R d The calculation formula is: Among them, L is the wheelbase, L f Indicates the distance between the front wheel and the front end of the vehicle, L r It indicates the distance between the rear wheels and the rear end of the vehicle, and W indicates the width of the vehicle.

4. The automatic parking trajectory planning method based on adaptive collision avoidance constraints according to claim 3 is characterized in that: In step S3, the adaptive collision constraint further includes: in a narrow channel, the standard for detecting the vehicle driving environment is as follows: Among them, x max ,y max is the maximum value of the horizontal and vertical coordinates of the drivable area, x min ,y min is the minimum value of the horizontal and vertical coordinates of the drivable area, N s To N s+i The representative trajectory point is in a narrow channel; Get the coordinates of the four corner points of the target vehicle's center position and connect them into a rectangle to perform collision detection with the surrounding polygonal obstacles. If there is no intersection between the two polygons, it means that there is no collision between the vehicle and the obstacle. Expand in four directions through each discrete point of the rectangle to obtain the maximum drivable area. The coordinates of the four vertices of the vehicle are calculated as follows: Among them, A x ,B x ,C x ,D x ,A y ,B y ,C y ,D y Respectively represent the horizontal and vertical coordinates of the four vertices of the rectangular vehicle, L f Indicates the distance between the front wheel and the front end of the vehicle, L r It indicates the distance between the rear wheels and the rear end of the vehicle, and W indicates the width of the vehicle; The adaptive collision avoidance constraint is expressed as follows: Among them, x max ,y max is the maximum value of the horizontal and vertical coordinates of the drivable area, x min ,y min is the minimum value of the horizontal and vertical coordinates of the drivable area, x adp ,y adp Represents the horizontal and vertical coordinates of the adaptive drivable area, N s To N s+i Indicates that the trajectory point is in a narrow channel.

5. The automatic parking trajectory planning method based on adaptive collision avoidance constraints according to claim 4 is characterized in that: In step S3, setting the cost function according to the parking time and comfort level of the target vehicle includes: The cost function is defined as follows: Among them, μ1>0, μ2>0 and μ3>0 represent weighting parameters, t f Penalize the parking time, acceleration ||ν(k+1)-ν(k)|| 2 Angular velocity||θ(k+1)-θ(k)|| 2 Penalize the smoothness and comfort of the parking trajectory.

6. The automatic parking trajectory planning method based on adaptive collision avoidance constraints according to claim 5 is characterized in that: In step S3, establishing an optimal control strategy in combination with the adaptive collision constraint and solving the final feasible trajectory to determine the optimal trajectory includes: A while loop is introduced to construct an intermediate optimal control problem to iteratively optimize the final feasible trajectory. The nonlinear vehicle kinematic constraints and geometric constraints are softened as external penalty costs and put into the cost function so that the kinematic constraints and geometric constraints meet the requirements, and then the iteration is stopped, as shown in the following formula: min J+ω p ·(J (1) +J (2) ) Among them, ω p represents the weight parameter, J (1) and J (2) Represent the corresponding penalties for kinematic constraints and geometric constraints respectively; In each loop iteration, the driving corridor is regenerated based on the optimal trajectory obtained last time, and the optimal trajectory in the current corridor is solved by the primal-dual interior point solver.

7. A system for implementing the automatic parking trajectory planning method based on adaptive collision avoidance constraints as described in any one of claims 1 to 5, characterized in that: The system comprises: A coarse path generation module is used to obtain the environmental information of the target vehicle to generate a grid map, and generate a two-dimensional feasible path on the grid map through the A* algorithm and a preset node generation algorithm, as well as a set of starting points, intermediate nodes, and end points; A singular point removal module is used to connect all points in the set using a hybrid A* algorithm. During the hybrid A* connection process, if it is detected that the time taken to connect a certain adjacent intermediate node is too long, or the parking operation mode is repeatedly changed, the intermediate node is deleted, and the final feasible trajectory is generated after traversing the set; The trajectory determination module is used to determine the adaptive collision constraints of the target vehicle based on the vehicle kinematic constraints and surrounding environmental factors, set the cost function according to the parking time and comfort of the target vehicle, establish the optimal control strategy in combination with the adaptive collision constraints, and solve the final feasible trajectory to determine the optimal trajectory.

Images