Multi-section parallel parking path planning method

Abstract

The invention provides a multi-section parallel parking path planning method, which comprises the following steps of: based on an Ackerman steering principle, establishing a vehicle kinematics model as a whole vehicle reference model of an AUV (Autonomous Underwater Vehicle), setting an obstacle avoidance safety distance for a vehicle, and simplifying the outer contour of the vehicle into a rectangle through maximum length and width parameters; setting a starting point, a temporary point and a target point, and generating a multi-section target path between the starting point and the temporary point and between the temporary point and the target point based on a preset multi-section path planning mode; and respectively setting an optimization target for each section of target path, converting into a nonlinear programming problem, and solving an optimal path through a particle swarm algorithm with a penalty function. According to the invention, the problems of parking difficulty, low parking efficiency and unsmooth path in a scene with limited parking space can be solved.

Description

Technical Field

[0001] The present invention relates to the field of intelligent driving technology, and in particular to a multi-segment parallel parking path planning method. Background Art

[0002] With the continuous development of my country's social economy, parking space resources have become scarce, urban parking spaces have become increasingly narrow, and parking operations have become more difficult. Automatic parking technology can solve this problem very well. Parking path planning is an important part of automatic parking and directly affects the success of automatic parking.

[0003] The current parking planning methods mainly include methods based on curve fitting. Curve fitting mostly uses polynomial curves, segmented curves and B-spline curves to design parking paths. Its design is simple, the calculation time is short, and the numerical optimization calculation ability is strong. There are also methods based on graph search, sampling and intelligence, such as hybrid A* algorithm, RRT algorithm, genetic algorithm and neural network algorithm to design parking paths and realize parking operations.

[0004] The current parallel parking path planning methods are mostly for scenarios with unlimited parking space, but cannot be applied to scenarios with limited parking space. The environment requirements are high. In addition, the curvature of the planned path is discontinuous and the path is not smooth. The parking process requires frequent on-the-spot steering, which causes tire wear and increases the working pressure of the electric power steering, which impairs parking efficiency and passenger comfort. Therefore, how to plan a path with continuous curvature under the premise of limited parking space is a problem that needs to be solved. Summary of the invention

[0005] The purpose of the present invention is to provide a multi-segment parallel parking path planning method, which can solve the problems of difficult parking, low parking efficiency and uneven path in a scene with limited parking space.

[0006] In order to achieve the above object, the technical solution adopted by the present invention is: a multi-segment parallel parking path planning method, comprising the following steps: Step 1: Based on the Ackerman steering principle, a vehicle kinematic model is established as the whole vehicle reference model of the AUV, an obstacle avoidance safety distance is set for the vehicle, and the outer contour of the vehicle is simplified into a rectangle through the maximum length and width parameters; Step 2: Set the starting point, temporary point and target point, and generate a multi-segment target path between the starting point and the temporary point, and between the temporary point and the target point based on the preset multi-segment path planning method; Step 3: Set optimization goals for each target path segment, transform it into a nonlinear programming problem, and solve the optimal path using a particle swarm algorithm with a penalty function.

[0007] Preferably, the whole vehicle reference model is: in, is the speed of the rear axle center point in the X-axis direction, is the speed of the rear axle center point in the Y-axis direction, φ is the vehicle heading angle, δ is the steering angle of the front axle center point, and L is the wheelbase.

[0008] Preferably, the coordinates of the four vertices of the vehicle outer contour are: Among them, L r is the vehicle rear overhang length, L f is the front overhang length and W is the vehicle width.

[0009] Preferably, the multi-segment target path includes dividing the parking path into a preparation segment path and a parking segment path, the preparation segment path trajectory borrows the parking space, including starting from a clothoid curve trajectory, followed by connecting two arc trajectories by a clothoid curve combination trajectory, and the arc trajectory extends to the temporary point; the parking segment path includes starting from a clothoid curve combination trajectory, followed by connecting two arc trajectories by a clothoid curve combination trajectory.

[0010] Preferably, the coordinates (xG, yG) and the azimuth angle αG of a point with a curvature ρ on the clothoid curve are: The trajectory of the clothoid curve is calculated by the above formula, where K is the power series expansion number, k is the curvature change rate of the clothoid curve, o is the initial fixed origin, and 0 is the initial azimuth.

[0011] Preferably, the coordinates of the point (xca, yca) with an azimuth angle θ on the arc curve segment whose starting position is the origin O of the geodetic coordinate system and whose initial azimuth angle is zero are: R is the radius of the arc curve segment.

[0012] Preferably, the coordinate conversion formula between the clothoid coordinate system xoy and the geodetic coordinate XOY at point G is: The coordinate transformation matrix

[0013] Preferably, the optimization target of the preparation segment path in step 3 is set as follows: the length of the rear axle center point mapped on the Y axis between the starting point and the temporary point is minimized, in order to ensure that the planned path occupies a minimum parking road space Ld; the optimization target of the parking segment path is set as follows: the coordinate of the rear axle center point mapped on the Y axis is minimized when the vehicle travels to the target point.

[0014] Preferably, the calculation formula of the particle swarm algorithm with penalty function is: Among them, f γ (x γ )(γ=1,2) is the objective function, As constraints, represents the search space, m γ is the number of constraints.

[0015] Preferably, the preparation segment path optimization target is set as follows: the length of the rear axle center point mapped on the Y-axis between the vehicle at the starting point and the temporary point is minimized; the parking segment path optimization target is set as follows: the coordinate of the rear axle center point mapped on the Y-axis is minimized when the vehicle travels to the target point.

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

[0017] 1. This solution uses multi-segment path planning and parking space to effectively reduce the requirements for the width of the parking space and the parking channel, thereby improving the passability of automatic parking path planning.

[0018] 2. This solution combines the clothoid curve trajectory with the circular arc curve trajectory, and uses the clothoid curve combined trajectory to connect the two circular arc curve trajectories, so that the path curvature is continuous, which improves the parking smoothness and reduces tire wear and steering assist pressure.

[0019] 3. This scheme uses the particle swarm algorithm with penalty function to transform the path planning problem into a nonlinear planning problem, so as to solve the optimal path, improve the path quality and reduce the planning time. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0021] Figure 1 This is an example diagram of a vehicle kinematics model based on the Ackerman steering principle of the present invention.

[0022] Figure 2 This is an example diagram of the clothoid curve combination of the present invention.

[0023] Figure 3 This is an example diagram of the preparatory parking path of the present invention.

[0024] Figure 4 This is an example diagram of the parking path for the parking section of the present invention.

[0025] Figure 5 This is an example diagram of the multi-segment parallel parking path of the present invention. DETAILED DESCRIPTION

[0026] The present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments.

[0027] The present invention discloses a multi-segment parallel parking path planning method. Figure 1-5 As shown, the embodiment includes the following steps.

[0028] Step 1: Figure 1 As shown in the figure, based on the Ackerman steering principle, a vehicle kinematic model is established as the whole vehicle reference model of the AUV:

[0029] in, is the speed of the rear axle center point in the X-axis direction, is the speed of the rear axle center point in the Y-axis direction, is the vehicle heading angle, δ is the steering angle of the front axle center point, and L is the wheelbase.

[0030] Set the obstacle avoidance safety distance for the vehicle, and simplify the vehicle's outer contour into a rectangle through the maximum length and width parameters. The coordinates of its four vertices are:

[0031] Among them, Lr is the rear overhang length of the vehicle, Lf is the front overhang length, and W is the vehicle width.

[0032] Step 2: Set the starting point, temporary point and target point, and generate a multi-segment target path between the starting point and temporary point, and between the temporary point and target point based on the preset multi-segment path planning method. Figure 5 shown.

[0033] The parking path is divided into a preparation path and a parking path. The preparation path uses the parking space, including starting from a clothoid curve trajectory, followed by two arc curve trajectories with different center points, and then connected by a clothoid curve combined trajectory. The arc trajectory extends to the temporary point. Figure 3 shown.

[0034] When the vehicle reaches the temporary point, the preparation section path ends. At this time, the vehicle's direction changes from forward to reverse, and then the parking section path driving operation is performed.

[0035] The parking segment path includes a clothoid curve combination trajectory starting from, followed by two arc curve trajectories with different center points, then the clothoid curve combination trajectory connects the two arc trajectories, and the tail segment is formed by the clothoid curve trajectory. When the vehicle reaches the target point, the front wheel turning angle of the vehicle is equal to zero. Figure 4 shown.

[0036] As a kind of transition curve, clothoid curve is mostly used in traffic roads and tracks. When the vehicle moves at a constant longitudinal speed and turning speed, the path curve is a clothoid curve. The clothoid curve combination is used to connect two arc paths to keep the overall curvature continuous.

[0037] Assume that the curvature change rate of the clothoid curve is k, the initial point is the fixed origin o, the initial azimuth is 0, and the coordinates (xG, yG) and azimuth αG of a point with curvature ρ on the clothoid curve are:

[0038] Where K is the power series expansion number, K = 15. The trajectory of the clothoid curve is calculated by the above formula.

[0039] Assuming the radius of the arc curve segment is R, the coordinates of the point (xca, yca) with an azimuth angle of θ on the arc curve segment whose starting position is the origin O of the geodetic coordinate system and whose initial azimuth angle is zero are: x ca =Rsinθ y ca =R(1-cosθ)

[0040] The arc trajectory is calculated by the above formula.

[0041] The clothoid curve combined trajectory is defined as a combination of a clothoid curve with an initial curvature of 0, a terminal curvature of 1 / Rmin, and a length of l and a clothoid curve rotated by an angle of π around its initial point. Rmin is the minimum turning radius of the vehicle, such as Figure 2 shown.

[0042] Since the vehicle heading angles of the same point in the geodetic coordinate system and the clothoid coordinate system are different, coordinate conversion is required. The coordinate conversion formula between the clothoid coordinate system xoy and the geodetic coordinate XOY at a certain point G is:

[0043] The coordinate transformation matrix

[0044] Step 3: Set optimization targets for the above-mentioned preparation segment path and parking segment path respectively, convert them into nonlinear programming problems, and solve the optimal path through the particle swarm algorithm with penalty function. The optimization target of the preparation segment path is set to minimize the length of the rear axle center point mapped on the Y axis between the vehicle at the starting point and the temporary point, in order to ensure that the planned path occupies the smallest parking road space Ld; the optimization target of the parking segment path is set to minimize the coordinate of the rear axle center point mapped on the Y axis when the vehicle drives to the target point, in order to ensure that the vehicle can enter the parking space deeper.

[0045] The calculation formula of the particle swarm algorithm with penalty function is:

[0046] Among them, f γ (x γ )(γ=1,2) is the objective function, As constraints, represents the search space, m γ is the number of constraints.

[0047] Combined with the constraints existing in each path segment, the inequality constraints Can be transformed into an equality constraint The above formula can be transformed into:

[0048] By setting the function Convert the constrained function minimization problem into an unconstrained function minimization problem:

[0049] Among them, F(x γ, M p ) is the penalty function, M p is the penalty factor, M q (x γ ) is the penalty term. p When F(x γ, M p ) will approach the optimal solution of the constrained problem.

[0050] In the particle swarm algorithm with penalty function, the penalty function allows each constraint to be processed separately and its violation degree is converted into an additional cost, so that the algorithm can comprehensively consider all constraints and find solutions that meet the constraints. It can effectively eliminate solutions that do not meet the constraints, avoid invalid searches, and thus improve the overall search efficiency. In addition, the introduction of the penalty function makes particles more inclined to approach the constraint boundary during the search process, avoiding the divergence of the search process caused by violation of constraints, thereby enhancing the convergence of the algorithm. It can flexibly adapt to constraints of different types and sizes, and enhance the adaptability of the algorithm in various constrained optimization problems.

[0051] This embodiment combines a clothoid curve combination with an arc trajectory to generate a multi-segment parallel parking path with continuous curvature, and optimizes each segment of the path through a particle swarm algorithm with a penalty function. By occupying parking space, the dependence on the width of the parking space channel is effectively reduced. The overall curvature of the parking path is continuous, which improves smoothness, reduces tire wear, and reduces the working pressure of the electric power steering.

[0052] It should be noted that the parts not described in detail in the above embodiments are all prior art.

[0053] The above description is only a preferred embodiment of the present invention and does not constitute any form of limitation to the present invention. Although the present invention has been disclosed as a preferred embodiment as above, it is not intended to limit the present invention. Any technician familiar with the profession can make some changes or modifications to equivalent embodiments of equivalent changes using the technical contents disclosed above without departing from the scope of the technical solution of the present invention. However, any simple modifications, equivalent changes and modifications made to the above embodiments based on the technical essence of the present invention without departing from the content of the technical solution of the present invention should be covered within the protection scope of the present invention.

Claims

1. A multi-segment parallel parking path planning method, characterized in that: The following steps are involved: Step 1: Based on the Ackerman steering principle, a vehicle kinematic model is established as the whole vehicle reference model of the AUV, an obstacle avoidance safety distance is set for the vehicle, and the outer contour of the vehicle is simplified into a rectangle through the maximum length and width parameters; Step 2: Set the starting point, temporary point and target point, and generate a multi-segment target path between the starting point and the temporary point, and between the temporary point and the target point based on the preset multi-segment path planning method; Step 3: Set optimization goals for each target path segment, transform it into a nonlinear programming problem, and solve the optimal path using a particle swarm algorithm with a penalty function.

2. A multi-segment parallel parking path planning method according to claim 1, characterized in that: The whole vehicle reference model is: in, is the speed of the rear axle center point in the X-axis direction, is the speed of the rear axle center point in the Y-axis direction, φ is the vehicle heading angle, δ is the steering angle of the front axle center point, and L is the wheelbase.

3. A multi-segment parallel parking path planning method according to claim 2, characterized in that: The coordinates of the four vertices of the vehicle outer contour are: Among them, L r is the vehicle rear overhang length, L f is the front overhang length and W is the vehicle width.

4. The multi-segment parallel parking path planning method according to claim 1, characterized in that: The multi-segment target path includes dividing the parking path into a preparation segment path and a parking segment path. The preparation segment path trajectory borrows the parking space, including starting from a clothoid curve trajectory, followed by connecting two arc trajectories by a clothoid curve combination trajectory, and the arc trajectory extends to the temporary point; the parking segment path includes starting from a clothoid curve combination trajectory, followed by connecting two arc trajectories by a clothoid curve combination trajectory.

5. The multi-segment parallel parking path planning method according to claim 4, characterized in that: The coordinates of a point on the clothoid curve with a curvature of ρ (x G ,y G ) and azimuth angle α G for: The trajectory of the clothoid curve is calculated by the above formula, where K is the power series expansion number, k is the curvature change rate of the clothoid curve, o is the initial fixed origin, and 0 is the initial azimuth.

6. A multi-segment parallel parking path planning method according to claim 4, characterized in that: The starting position is the origin O of the geodetic coordinate system and the point (x ca ,y ca ) is: x ca =Rsinθ y ca =R(1-cosθ) R is the radius of the arc curve segment.

7. The multi-segment parallel parking path planning method according to claim 1, characterized in that: The coordinate conversion formula between the clothoid coordinate system xoy and the geodetic coordinate XOY at point G is: The coordinate transformation matrix 8. The multi-segment parallel parking path planning method according to claim 1, characterized in that: The optimization target of the preparatory path in step 3 is set to: the length of the rear axle center point between the starting point and the temporary point mapped on the Y axis is minimized, in order to ensure that the planned path occupies the parking road space L d Minimum: The parking segment path optimization goal is set to the minimum coordinate of the rear axle center point mapped on the Y axis when the vehicle travels to the target point.

9. The multi-segment parallel parking path planning method according to claim 1, characterized in that: The calculation formula of the particle swarm algorithm with penalty function is: Among them, f γ (x γ )(γ=1,2) is the objective function, As constraints, represents the search space, m γ is the number of constraints.

10. The multi-segment parallel parking path planning method according to claim 1, characterized in that: The optimization target of the preparatory path is set as follows: the length of the rear axle center point mapped on the Y axis between the starting point and the temporary point is minimized; The optimization target of the parking segment path is set as follows: when the vehicle reaches the target point, the coordinate of the rear axle center point mapped on the Y axis is the smallest.

Images