An automatic parking path planning method based on four-wheel steering of a vehicle

Abstract

The application relates to the field of automatic driving path planning of an automobile, and discloses an automatic parking path planning method based on four-wheel steering of a vehicle. The planning method comprises the following steps: an initial path planning is carried out by using an improved RRT* Reeds-Sheep curve based on a parking scene and combined with four-wheel steering, so that an initial path containing vehicle position information and vehicle pose angle information is obtained; speed coarse planning is carried out on the obtained initial path, so that an initial trajectory is obtained; by establishing an optimal control problem, setting a vehicle system dynamic motion equation constraint based on four-wheel steering, a two-point boundary constraint and a manifold constraint, the optimal control problem is solved to obtain an optimized final trajectory, so that a subsequent parking trajectory tracking controller can carry out target trajectory tracking control. The automatic parking path planning method can realize parking in a narrow area, can realize dynamic obstacle avoidance in the parking process, and can improve the automatic parking maneuverability of the automobile.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of automatic driving path planning of automobile, and particularly relates to an automatic parking path planning method based on four-wheel steering of vehicle. BACKGROUND

[0002] In recent years, with the increasing of private car ownership, the problem of parking difficulty gradually highlights, in order to reduce the driving pressure of the driver, the automatic parking system has been widely used. At present, the automatic parking system mostly uses the common path planning and tracking control module in the automatic driving system as the scheme to realize the system function, and the planning quality of the parking path determines whether the vehicle tracking control module can realize the tracking of the path within the allowable deviation range. Therefore, when facing the more complex parking scene, planning the parking path planning that is suitable for the current scene and can meet the kinematic constraint requirements of the vehicle has important research significance for the development of the automatic parking system.

[0003] At present, there are many path planning methods applied in automatic parking, for example: using quintic polynomial to plan the path under parallel parking conditions to ensure the continuity of the curvature of the planned path; using Rapidly-exploring Random Trees (RRT) algorithm, starting from the starting point, expanding in a tree structure until the target point or the point near the target point is also expanded to the tree, at this time, a series of discrete points from the starting point to the target point can be found on the tree, and then the path planning process considering the kinematic constraint of the vehicle is guided through the established discrete points, thereby obtaining a path considering the kinematic constraint of the vehicle.

[0004] However, the above path planning methods mainly aim to realize the path planning from the starting point to the ending point, ignoring a series of kinematic system dynamic constraints of the vehicle, and with the gradual intensification of the phenomenon of parking space shortage, the above method cannot realize parking in a smaller space due to the minimum turning radius of the vehicle. In addition, the internal structure of the parking lot is relatively complex, the above method ignores the existence of dynamic obstacles and does not consider the obstacle collision constraint condition in the parking process, which may cause collision accidents of the vehicle in the parking process.

[0005] And the path planning method based on four-wheel steering, on the basis of considering the kinematics of the vehicle, the four-wheel steering vehicle has a smaller turning radius of the vehicle, and its characteristics can realize turning in a smaller space for further parking, and the vehicle with four-wheel steering has better steering response, so that the dynamic obstacles can be more easily avoided by establishing obstacle collision constraints, and real-time dynamic obstacle avoidance is realized.

[0006] Therefore, the application plans a path based on four-wheel steering, and can generate a trajectory that conforms to the kinematics of a vehicle, which can not only park in a narrow area, but also dynamically avoid obstacles during parking. SUMMARY

[0007] The application aims to provide an automatic parking path planning method based on four-wheel steering. The application aims to generate a trajectory that conforms to the kinematics of a vehicle, which can not only park in a narrow area, but also dynamically avoid obstacles during parking.

[0008] To achieve the above-mentioned purpose, the following technical solutions are adopted:

[0009] An automatic parking path planning method based on four-wheel steering, comprising:

[0010] Step one, using the RRT* algorithm combined with the Reeds-Sheep curve to plan an initial path, ensuring the probability completeness of the path planning process, realizing the feasible solution search of the parking trajectory under different starting positions and different types of parking scenes, and quickly excluding the sampling area that is impossible to appear based on prior knowledge, significantly improving the planning efficiency of the algorithm, and generating an initial path with continuous curvature containing vehicle position information and vehicle attitude angle information by using the improved algorithm.

[0011] Step two, performing speed rough planning on the initial path obtained in step one, first using the step length information in random search planning to obtain the speed information of each state point with the set sampling interval time DT Then, the initial trajectory state points corresponding to N+1 time points are obtained through downsampling, and an initial speed change curve corresponding to the initial path in step one is obtained.

[0012] Step three, regarding the automatic parking path planning problem as a nonlinear optimal control Bolza type problem, i.e., solving the continuous Bolza form optimal control problem, inputting the trajectory obtained in step two as the initial solution of the problem, determining the termination time t f and the control variable u(t), t∈[0,t f ] by establishing the system dynamic equation constraint based on four-wheel steering, two-point boundary constraint and flow constraint, so that the cost function J takes the minimum value under the premise of satisfying all the constraints along the corresponding state trajectory x(t).

[0013] Thus, the optimized final parking trajectory is obtained, which can be provided to the vehicle parking trajectory tracking controller for target parking trajectory tracking control.

[0014] The beneficial effects of the present application are:

[0015] 1. The automatic parking path planning method based on four-wheel steering, which uses an improved RRT* algorithm for initial path planning, the improved algorithm ensures the probability completeness of the path planning process and confirms the sampling area range according to different parking scenes, quickly excludes the sampling area that is impossible to appear based on prior knowledge, and significantly improves the planning efficiency of the algorithm.

[0016] 2. The automatic parking path planning method based on four-wheel steering, when the RRT* algorithm is used for initial path planning, the expansion mode of the node is updated to the Reeds-Shepp curve expansion mode based on four-wheel steering, so that the vehicle can have a smaller turning radius and can meet the parking in a smaller space.

[0017] 3. The automatic parking path planning method based on four-wheel steering, when the vehicle kinematics equation constraint is performed, the kinematics equation constraint of four-wheel steering is performed, so that the vehicle has a better steering response, and in combination with the established manifold constraint, real-time dynamic obstacle avoidance in the parking process can be realized. BRIEF DESCRIPTION OF DRAWINGS

[0018] Figure 1 The overall flowchart of the automatic parking path planning method based on four-wheel steering.

[0019] Figure 2 The RRT* Reeds-Shepp algorithm flowchart of step one in the automatic parking path planning method based on four-wheel steering.

[0020] Figure 3 The vehicle kinematics model diagram based on four-wheel steering in step three of the automatic parking path planning method based on four-wheel steering.

[0021] Figure 4 The triangular area method principle diagram in the manifold constraint in step three of the automatic parking path planning method based on four-wheel steering.

[0022] Figure 5 The initial path planning result of the implementation example of the automatic parking path planning method based on four-wheel steering.

[0023] Figure 6 The speed rough planning result diagram of the implementation example of the automatic parking path planning method based on four-wheel steering.

[0024] Figure 7A trajectory comparison chart before and after optimization of an implementation example of the four-wheel steering-based automatic parking path planning method described in the present application. DETAILED DESCRIPTION

[0025] The present application will be further described in detail below with reference to the accompanying drawings, so that those skilled in the art can implement the present application according to the description and the drawings.

[0026] As shown in the drawings, Figure 1 the overall flow of the four-wheel steering-based automatic parking path planning method described in the present application includes the following steps:

[0027] Step one, use the RRT* algorithm combined with Reeds-Sheep curve (i.e., RRT*RS algorithm) for initial path planning, which ensures the probability completeness of the path planning process, realizes the feasible solution search of the parking trajectory under different starting positions and different types of parking scenarios, and based on prior knowledge, quickly excludes the sampling area that is not possible to appear, significantly improves the planning efficiency of the algorithm, and generates an initial path with continuous curvature containing vehicle position information and vehicle attitude angle information using the improved algorithm.

[0028] Step two, speed coarse planning is performed on the initial path obtained in step one, first use the step length information in the random search planning and the set sampling interval time DT to obtain the speed information of each state point , and then obtain the initial trajectory state points corresponding to N+1 time points through downsampling, and further obtain an initial speed change curve corresponding to the initial path in step one.

[0029] Step three, the automatic parking path planning problem is regarded as a nonlinear optimal control Bolza type problem, i.e., the continuous Bolza form optimal control problem is solved, the trajectory obtained in step two is input as the initial solution of the problem, the system dynamic equation constraint based on four-wheel steering, two-point boundary constraint and flow constraint are established, the termination time t f is determined, as well as the control variable u(t), t∈[0,t f ], under the premise of satisfying all the constraints along the corresponding state trajectory x(t), the cost function J takes the minimum value.

[0030] Solving this optimization problem, the optimized final parking trajectory can be obtained, which is provided to the vehicle parking trajectory tracking controller to perform target parking trajectory tracking control, so that the four-wheel steering vehicle can automatically park in a narrow area and achieve the purpose and effect of dynamic obstacle avoidance during parking.

[0031] As shown in the drawings, Figure 2The specific algorithm flow of the RRT*RS algorithm in step one is obtained by improving the traditional RRT* algorithm as follows:

[0032] The expansion mode of the node is updated to the Reeds-Shepp curve expansion mode based on four-wheel steering; in order to ensure high operation efficiency of the algorithm, the Euclidean distance ignoring the vehicle heading angle information is used as the distance measurement in the nearest node search process; the function confirms the random sampling area according to different parking scenes; the starting node is added in the near node search process; the Reeds-Shepp curve length considering the cost term shown in formula (1) is applied in the parent node selection process; the expansion from the current node to the parking target pose is attempted at the end of each random node expansion cycle to improve the parking planning search speed, and the number of nodes connected to the parking target pose added in the search random tree is detected in the expansion process to determine whether the set number of feasible paths is reached, and if the set number of successful paths N suc is reached, the search process is ended in advance.

[0033]

[0034] In the formula, t, u, and v represent the radian or length of each circular arc or straight line after normalization; dir i represents the driving direction of the i-th path in the curve combination, and is set to 1 for the forward direction and -1 for the reverse direction; f(x) = 0 when x < 0, and f(x) = 1 in other cases.

[0035] The pseudo code of the improved RRT*RS algorithm is shown in Table 1.

[0036] Table 1 Pseudo code of the improved RRT*RS algorithm

[0037]

[0038] Figure 3 For the four-wheel steering vehicle kinematics model diagram in step three, combined with Figure 3 , the four-wheel steering vehicle kinematics model equation shown in formula (2) is established to describe the vehicle kinematics state:

[0039]

[0040] In the formula, δ f is the front wheel steering angle of the four-wheel steering vehicle; δ r is the rear wheel steering angle of the four-wheel steering vehicle; is the vehicle azimuth angle; L is the wheelbase; the horizontal coordinate displacement, the vertical coordinate displacement, and the speed of the rear axle midpoint are x r , y r , and v r respectively.

[0041] In step three, the two-point boundary constraints are established to provide the motion state of the vehicle at the initial time t = 0 and the final time t = t f :

[0042] When t = 0,

[0043]

[0044] In the formula, corresponding to the initial motion state information recorded by the vehicle-mounted sensor;

[0045] When t = t f , the vehicle is finally stably parked in a certain area:

[0046] υ i (t f ) = 0; a i (t f ) = 0; the wheel is required to be straightened at the end ω i (t f ) = 0; at the end time, a certain predetermined attitude angle sin(θ i (t f )) = sin(θ fi ) and cos(θ i (t f )) = cos(θ fi ) is reached; according to the coordinates of the midpoint of the rear axle of the vehicle, the vehicle width, the front suspension distance of the vehicle, the rear suspension distance of the vehicle, the wheelbase, the attitude angle, etc., the coordinates of each vertex of the vehicle can be obtained, and according to the straight lines on which the four edges of the rectangular area formed by the four vertices of the parking space lie, it is determined which side of the straight line each vertex of the vehicle is located on, so as to constrain x i (t f ), y i (t f ), and θ i (t f ).

[0047] In step three, the established popular constraints are mainly to limit the control variables u(t) and the state variables x(t) on a certain high-dimensional curved surface in the solution space or within the area enveloped by the curved surface. Specific limitations include: motion ability limitation and external environment limitation;

[0048] For the motion ability limitation, it can be expressed by the following relationship,

[0049]

[0050] In the formula, Φ fmax represents the maximum allowed deflection angle value of the front wheel steering angle of the vehicle; Φrmax This represents the maximum permissible deflection angle of the vehicle's rear wheels; v max This is the upper limit of the safe driving speed for vehicles in low-speed scenarios; to ensure passenger comfort, a max Ω fmax Ω rmax These are the amplitude settings for linear acceleration, front wheel angular velocity, and rear wheel angular velocity, respectively.

[0051] In the manifold constraints of step three, the following external environmental constraints must be satisfied:

[0052] Collision avoidance constraints are established between the vehicle's rectangular body and the convex polygon obstacle. The "triangle area method" is used to ensure that at every moment, the vertices of the vehicle's rectangular body are outside the obstacle polygon, and the vertices of the obstacle are outside the vehicle's rectangular body. This is used to determine the x-axis. i (t f ), y i (t f ), θ i (t f (This can be constrained.)

[0053] The principle of the "triangle area method" is as follows: Figure 4 As shown, taking a pentagon as an example with vertices Q1, Q2, Q3, Q4, and Q5 as the obstacle, point P is connected to each pair of adjacent vertices of the convex polygon to form triangles. The areas of these triangles are then summed. If the sum of the areas is greater than that of the convex polygon, then point P is outside the convex polygon; otherwise, point P is located on one side of the polygon or inside the convex polygon. Point P is located on the convex polygon Q1~Q5. n The external constraints are:

[0054]

[0055] In the formula, S Δ S represents the area of ​​the corresponding triangle; Y S represents the area of ​​a convex polygon; Δ The solution should be obtained using the coordinates of the triangle vertices. For example, suppose P = (x, y) and Q = (y, y). k =(x Qk ,y Qk ), Q k+1 =(x Q(k+1) ,y Q(k+1) ),but

[0056]

[0057] In step three, the established cost function serves as an indicator for selecting high-quality trajectories:

[0058] J = ω1J1 + ω2J2 + ω3J3 + ω4J4 # (7)

[0059] In the formula, ω1, ω2, ω3, ω4 are weight coefficients of each performance index; J1 = t f , which means that the vehicle is expected to move to a state that can meet the termination time constraint as soon as possible; which summarizes the energy consumed by the vehicle i due to wheel angle changes and speed changes in the entire motion time domain, for representing the smoothness of the vehicle trajectory; for representing the appropriate distance of the vehicle trajectory from the obstacle j, the coefficient κ j > 0 determines the degree of the vehicle's expectation to move away from the obstacle j, N obs is the number of convex polygon obstacles, for describing the distance between the vehicle i and the obstacle j, and the geometric center of the obstacle is G j = (x Gi , y Gi ).

[0060] Next, an example is used to demonstrate the application process and planning effect of the method proposed in the present application.

[0061] The relevant parameters of the vehicle in this example: front suspension L f = 0.985 m, rear suspension L r = 0.885 m, wheelbase L = 2.625 m, vehicle width L b = 1.82 m; combined with the relevant parameters of the vehicle, the manifold constraint motion ability limit in step three: a max = 0.3 m / s 2 , v max = 1.4 m / s, w max = 0.5 rad / s 2 , Φ fmax = 35°, Φ rmax= 5°; Step three static obstacle boundary information in the manifold constraint: straight line composed of [-10.0, 1.2], [-3.0, 1.2], [-3.0, 1.2], [-3.0, -1.5], [-3.0, -1.5], [4.0, -1.5], [4.0, -1.5], [4.0, 1.2], [4.0, 1.2], [10.0, 1.2], [-10.0, 6.0], [10.0, 6.0], dynamic obstacle displayed on the map as a 1x1 rectangle, with the right bottom corner of the rectangle as the reference vertex, the starting coordinate being (5, 4), the end coordinate being (-3, 4), and the moving step being 0.2; vehicle starting point position information and pose angle: [-4.0, 3.0, 0.0], vehicle end point position information and pose angle: [-1.0, 0.0, 0.0], the RRT*RS algorithm described in Step one is used to explore with each information as input, and the planned path is as shown in Figure 5 As shown in

[0062] As shown in Figure 6 the speed of the vehicle during the parking process is set to remain unchanged at 0.8 m / s (when the vehicle moves forward, the speed is 0.8 m / s; when the vehicle moves backward, the speed is -0.8 m / s), and the speed change curve, in combination with the initial path planned in Step one, can obtain an initial trajectory containing speed information.

[0063] The initial trajectory obtained in Step two is input as the initial solution, the system dynamic equation constraint based on four-wheel steering, the two-point boundary constraint, and the manifold constraint are established through the known data of the example and the above method, and the optimized trajectory is obtained, and the comparison between the initial trajectory (as shown by the short dashed line in the figure and the vehicle body contour pose) and the optimized trajectory (as shown by the dotted line in the figure and the vehicle body contour pose) is as shown in Figure 7 As shown in

[0064] Although the embodiments of the present application have been disclosed as above, they are not limited to the application listed in the specification and the embodiments, and can be fully applied to various fields suitable for the present application, and other modifications can be easily realized by those skilled in the art, and therefore the present application is not limited to specific details and the figures shown and described herein, without departing from the general concept defined by the claims and the equivalent scope.

Claims

1. A method for automatic parking path planning based on vehicle four-wheel steering, characterized in that, Comprise: Step one, the traditional RRT* algorithm is improved to complete the preliminary path planning, while ensuring the probability completeness of the path planning process, the feasible solution search of the parking trajectory under different starting positions and different types of parking scene is realized, and based on the prior knowledge, the sampling area that is impossible to appear is quickly excluded to improve the planning efficiency of the algorithm, and an initial path with continuous curvature containing vehicle position information and vehicle attitude angle information is generated by using the improved algorithm; Step two, speed rough planning for the initial path obtained in step one, using the step length information in random search planning The speed information of each state point is obtained at the set sampling interval time DT The initial trajectory state points corresponding to N+1 time points are obtained by downsampling, and then an initial speed change curve corresponding to the initial path in step one is obtained Step 3: Treat the automatic parking path planning problem as a nonlinear optimal control Bolza-type problem, i.e., solve a continuous Bolza-form optimal control problem. Use the initial trajectory obtained in Step 2 as the initial solution to the problem. Determine the termination time t by setting constraints on the vehicle system dynamic equations based on four-wheel steering, two-point boundary value constraints, and flow constraints. f and control variable u(t), t∈[0,t f Under the premise of satisfying all constraints along the corresponding state trajectory x(t), the cost function J is minimized.

2. The automatic parking path planning method based on vehicle four-wheel steering according to claim 1, wherein, In the step one, the traditional RRT* algorithm is improved in the following specific method: The node expansion mode is updated to the Reeds-Shepp curve expansion mode based on four-wheel steering; In order to ensure that the algorithm has high operation efficiency, the Euclidean distance ignoring the vehicle heading angle information is used as the distance measure in the nearest node search process; The function confirms the random sampling area according to different parking scenes; The starting node is added in the near node search process; The Reeds-Shepp curve length considering the cost term is applied in the parent node selection process; At the end of each random node expansion loop, the system attempts to expand towards the parking target pose from the current node to improve parking planning search speed. During the expansion process, the number of nodes connected to the parking target pose that have been added to the search random tree is checked to determine whether the set number of feasible paths has been reached. If the specified number of successful paths N is reached, the system proceeds. suc The search process will end prematurely.

3. The automatic parking path planning method based on vehicle four-wheel steering according to claim 1, wherein, In the step three, the vehicle kinematics model based on four-wheel steering is established to describe the kinematics properties of the vehicle to constrain the vehicle system dynamic equation: where δ f is the front wheel steering angle of the four-wheel steering vehicle; δ r is the rear wheel steering angle of the four-wheel steering vehicle. is the vehicle orientation angle; L is the wheel base; and the horizontal coordinate, vertical coordinate and speed of the rear axle midpoint are x r , y r , v r , respectively.

4. The automatic parking path planning method based on vehicle four-wheel steering according to claim 1, wherein, In the third step, the two-point boundary conditions are used to provide the motion state of the vehicle at the initial time t = 0 and the final time t = t f , in particular by When t=0, In the formula, corresponding to the initial motion state information recorded by the vehicle-mounted sensor; When t = t f the vehicle is finally parked stably in a certain area, at which time: v i (t f ) = 0, a i (t f ) = 0; require the wheel to be back to the original position at the end ω i (t f ) = 0; at the end, reach a certain attitude angle sin(θ i (t f )) = sin(θ fi ), cos(θ i (t f )) = cos(θ fi ); according to the midpoint coordinates of the rear wheel axle of the vehicle, the vehicle width, the front suspension distance of the vehicle, the rear suspension distance of the vehicle, the wheelbase, the attitude angle, etc., the coordinates of each vertex of the vehicle can be obtained, and according to the straight lines where the four edges of the rectangular area formed by the four vertices of the parking space are located, it is determined which side of the straight line each vertex of the vehicle is located on, so as to constrain x i (t f ), y i (t f ), θ i (t f ).

5. The automatic parking path planning method based on vehicle four-wheel steering according to claim 1, wherein, In the step three, the popular constraint is mainly to limit the control variable u(t) and the state variable x(t) in a certain high-dimensional surface or the area enveloped by the surface in the solution space, and the specific restrictions include: movement ability restriction and external environment restriction; The movement ability restriction satisfies: wherein Φ fmax represents the maximum allowable deflection angle value of the front wheel steering angle of the vehicle; Φ rmax represents the maximum allowable deflection angle value of the rear wheel steering angle of the vehicle; v max is the upper limit of the safe driving speed of the vehicle in the low-speed scenario; a max , Ω fmax , Ω rmax are the amplitudes of the linear acceleration, the front wheel steering angle velocity, and the rear wheel steering angle velocity, respectively; The external environment restriction satisfies: collision avoidance constraint conditions between the vehicle body rectangle and the convex polygon obstacle are established, and x i (t f ), y i (t f ), and θ i (t f ) are constrained by a triangular area method.

6. The automatic parking path planning method based on vehicle four-wheel steering according to claim 2, wherein, In the step one, the cost term considering the cost term is applied in the parent node selection process: where t, u, v represent the radian or length of each arc or straight line after normalization; dir i f(x) represents the driving direction of the i-th path in the curve combination, where the positive direction is 1 and the reverse direction is -1; f(x) = 0 when x < 0, and f(x) = 1 in other cases.

7. The automatic parking path planning method based on vehicle four-wheel steering according to claim 5, wherein, In the step three, the principle of the triangular area method is: Each vertex of the vehicle and each two adjacent vertices of the convex polygon respectively form a triangle, the triangular areas are accumulated, and if the sum of the accumulated sum is greater than the area of the convex polygon, the vertex is outside the polygon, so as to ensure that each time the vehicle body rectangle vertex is outside the obstacle polygon, and the obstacle vertex is outside the vehicle body rectangle.

Images