CN113830079A - Online planning method and system for continuous curvature parking path with any initial pose - Google Patents

Abstract

The invention relates to an online planning method and system for a continuous curvature parking path with any initial pose, wherein the online planning method adopts a sectional path planning idea to obtain a parking path, the path planning process is divided into a warehouse location internal adjustment path planning and a warehouse entry path planning, the warehouse location internal adjustment path planning comprises a first warehouse location internal adjustment path planning used for obtaining a warehouse entry target pose and a second warehouse location internal adjustment path planning used for enabling the vehicle pose to meet the final pose requirement, the warehouse location internal adjustment path planning adopts an optimization method to carry out section-by-section planning, and the warehouse entry path planning adopts an improved Hybrid A algorithm to plan to obtain a warehouse entry path from the current pose of a vehicle to the warehouse entry target pose. The automatic parking system can be used for re-planning and re-tracking by adopting the method. Compared with the prior art, the invention has the advantages of high calculation efficiency, improved parking performance, capability of parking in any storage position (parallel, vertical and oblique), and the like.

Description

Online planning method and system for continuous curvature parking path with any initial pose

Technical Field

The invention relates to the field of vehicle control, in particular to a parking control method, and particularly relates to an online planning method and system for a continuous curvature parking path with any initial pose.

Background

The automatic parking technology has come to the fore, and the automobile keeping quantity is increased year by year, so that the urban parking space is increasingly narrow, and the automatic parking technology can help a driver to safely and efficiently park. The automatic parking path planning is used as a key technology for planning an executable path connecting the current pose of the vehicle and the pose of an expected target, so that the vehicle has no collision and avoids pivot steering in the tracking control process. Although there are many path planning related researches at present, the parking scene still has the problems of low efficiency and accuracy and the like.

Currently popular path planning methods can be divided into four categories: geometric methods, graph search methods, sampling-based methods, and optimization-based methods. The geometric method uses a curve group, a polynomial curve, a Bezier curve, a B spline curve and the like to connect the initial pose and the target pose, the planning path is smooth and the calculation efficiency is high, but the planning is not flexible due to the adoption of a given form of curve planning, and the planning of any initial pose is difficult to meet. The Hybrid A algorithm in the graph search method is widely applied to parking scenes, the method solves the problem that any initial pose can be planned, the calculation efficiency under partial poses is low, and the path quality obtained by solution is unstable. The Bi-RRT algorithm based on RS curve expansion in the sampling method can be used for parking path planning, but the planning efficiency of any initial pose is difficult to reliably ensure due to the random mechanism in the sampling method. The parking problem is modeled into a nonlinear programming problem based on an optimization method, and then iterative solution is carried out in a numerical optimization mode, the quality of a planned path is high, but the whole parking process is difficult to directly model into a convex optimization problem, and for the non-convex optimization problem, the initial value greatly affects the solution success rate and the calculation efficiency.

Disclosure of Invention

The invention aims to overcome the defects that the prior art cannot give consideration to any parking starting pose, path quality and calculation efficiency, and provides a continuous curvature parking path online planning method and system with any starting pose so as to improve the parking reliability.

The purpose of the invention can be realized by the following technical scheme:

a parking path is obtained by adopting a sectional path planning idea, the path planning process is divided into a warehouse location internal adjustment path planning and a warehouse entry path planning, the warehouse location internal adjustment path planning comprises a first warehouse location internal adjustment path planning used for obtaining a warehouse entry target pose and a second warehouse location internal adjustment path planning used for enabling the vehicle pose to meet the final pose requirement, the warehouse location internal adjustment path planning adopts an optimization method to carry out section-by-section planning, and the warehouse entry path planning adopts an improved Hybrid A algorithm planning to obtain a warehouse entry path from the current pose of the vehicle to the warehouse entry target pose.

Further, the implementation of the in-library adjustment path planning by using an optimization method specifically includes:

modeling the in-library adjustment path planning problem to solve the optimization problem of the CC circular curvature and the arc length, wherein the constructed optimization model is expressed as follows:

s.t.q_j+1＝f_e(d，q_j，u_j)

h_p(q_j+1)≤0

u_min≤u_j≤u_max

wherein the objective function l_O(q_j+1)＝|θ_j+1-θ_GI is the vehicle heading angle theta_j+1Angle theta with target course_GDifference of (q)_j+1Adjusting the state q in path planning for in-bin_jState after passing through a section of CC circle, u_jFor control input, u_minAnd u_minFor controlling the minimum and maximum values of the input, the constraint h_pRepresenting environmental constraints, and d ∈ { -1, 1} is the vehicle motion direction.

Further, the CC circle is a continuous curvature arc formed by connecting a clothoid curve, an arc, and a straight line.

Further, the first in-reservoir adjustment path planning adopts a method of gradually planning from the final pose to the outside by a CC circle with the minimum radius until the requirement that the vehicle can be delivered from the reservoir without collision is met, and the warehousing target pose is obtained.

And further, the second storehouse position inner adjustment path planning adopts a method of gradually planning from the current position posture to the outside by a CC circle with the minimum radius until the requirement that the vehicle can be delivered from the storehouse without collision is met, the adjustment path in the next section of storehouse position of the current position posture is obtained, and re-planning and tracking are carried out after the adjustment path planning in each section of storehouse position is finished until the vehicle position posture meets the requirement of the final position posture.

Further, the step of obtaining the warehousing path from the current pose of the vehicle to the warehousing target pose by adopting the improved Hybrid a algorithm planning specifically comprises the following steps:

designing a continuous curvature curve group based on a CC circle, selecting a path in the same form curve group according to a shortest path principle, and selecting a path according to a principle of minimum gear shifting times for different forms of curve groups;

and expanding the state nodes in a continuous curvature mode based on the continuous curvature curve group, measuring the importance degree of the expanded state nodes based on the designed evaluation function, and taking the node with the minimum evaluation function value as a priority expansion node in the searching process to obtain a final warehousing path.

Further, the valuation function f (j) is expressed as:

f(j)＝g(j)+h(j)

wherein, the cost item function g (j) is used for measuring the parking starting state node to the intermediate state node q_jThe heuristic function h (j) is an intermediate state node q_jCost estimate to target state node for q metric_jProximity to a target state.

Further, the cost term function g (j) is expressed as:

g(j)＝g_l(j)+g_rev(j)+g_κ(j)+g_Δκ(j)

g_l(j)＝k_l·Δl

g_κ(j)＝k_κ·|κ_j|

Δκ_j＝κ_j-κ_j-1，Δκ_j-1＝κ_j-1-κ_j-2

wherein the cost term function g (j) is composed of a path length term g_l(j) Shift penalty term g_rev(j) Large curvature penalty term g_κ(j) And a curvature change penalty term g_Δκ(j) Composition k_lIs the path length term coefficient; k is a radical of_revIs a shift penalty term coefficient; k is a radical of_κPunishment coefficient for path large curvature; k is a radical of_Δk1、k_Δκ2Penalizing the coefficient of the term for curvature change, and k_Δκ1＞k_Δκ2；κ_jIs the curvature,. DELTA.l is the change in arc length, d_jFor the direction of movement of the vehicle, Δ κ_jIs a change in curvature.

Further, the heuristic function h (j) is expressed as:

h(j)＝k_nonholo·h_nonholo(j)+k_shift·h_shift(j)

+k_holo·h_holo(j)

wherein the heuristic function h (j) is derived from the shortest path heuristic h_nonholo(j) Minimum path segment number heuristic h_shift(j) And obstacle avoidance elicitor h_holo(j) Composition of h_nonholo(j) In order to take into account only environmental obstacles and not the path length from each grid point to the target point when the vehicle is not constrained by integrity, h_nonholo(j) And h_shift(j) When the environmental barrier is not considered, the length of the continuous curvature path from each grid point to the target pose and the number of the path segments are calculated by the continuous curvature curve group.

The invention also provides a continuous curvature automatic parking system with any initial pose, which comprises:

the path online planning module is used for obtaining a parking path by adopting the online planning method;

and the tracking module executes parking based on the parking path, tracks the parking track, judges whether collision risks exist or whether a global obstacle map is updated in real time, and calls the path online planning module to perform re-planning until parking and warehousing if the collision risks exist or the global obstacle map is updated, and if the collision risks exist or the global obstacle map is updated, the path online planning module is called to perform re-planning.

Compared with the prior art, the invention has the following beneficial effects:

1. the invention gives consideration to any parking starting pose, path quality and calculation efficiency, and the arbitrary starting pose can be planned to help improve the success rate of parking; the high path quality means that the path length is short, the number of direction changes is small, the safety requirement is met, the smoothness is good, and the parking efficiency, the parking experience and the path tracking precision are improved; the high calculation efficiency contributes to reduction of the parking planning waiting time. The path planning method provided by the invention can be directly applied to an automatic parking system with re-planning capability, and is beneficial to improving the parking performance.

2. The whole parking path planning is divided into two parts of a warehouse location internal adjustment process and a warehouse entry process path planning and solved respectively, the warehouse location internal adjustment process is converted into a convex optimization problem of solving the curvature and the arc length of a continuous curvature arc and is solved by adopting an internal point method, and the path quality and the calculation efficiency are high; and the route planning in the warehousing process adopts an improved Hybrid A algorithm to expand state nodes in a continuous curvature form, and an executable route with continuous curvature reaching a target pose can be directly generated at any initial pose without post-processing, so that the calculation efficiency is high.

3. The invention designs the shortest path heuristic item and the minimum path segment number heuristic item aiming at the parking problem so as to further improve the calculation efficiency.

4. According to the method, a cost item function considering the path length, the direction change times and the smoothness is designed, the parking path is segmented according to curve types to obtain a vehicle driving area polygon, then a quadrant method is adopted for collision detection to ensure the quality of a planned path, and through the design of a gear shift punishment item in the cost item function and a gear shift time estimation item in an enlightening item function, the search is effectively converged to a state point with less gear shift times, and finally a planning result with less path segments is obtained.

Drawings

FIG. 1 is a schematic diagram of a parallel, vertical library position scene and a target pose;

FIG. 2 is a schematic flow chart of a path planning method according to the present invention;

FIG. 3 is a schematic view of a kinematic model of a vehicle;

FIG. 4 is a schematic diagram of a transition curve, which is (4a) a curvature change of the transition curve, (4b) a course angle change, and (4c) a curve shape;

FIG. 5 is a schematic diagram of a CC circle, wherein (5a) is the variation of curvature of the CC circle and (5b) is the shape of the curve;

FIG. 6 is a schematic diagram of the in-warehouse location adjustment planning process before warehouse entry path planning;

FIG. 7 is a schematic diagram of the in-warehouse location adjustment planning process after the warehousing path tracking;

FIG. 8 is a schematic diagram of a combination of five continuous curvature curves;

FIG. 9 is a schematic view of a continuous curvature state node expansion;

FIG. 10 is a schematic view of a vehicle tracking a travel area along different paths, wherein (10a) is a straight path, (10b) is a circular arc path, (10c) is a cloothoid path with increasing curvature, and (10d) is a cloothoid path with decreasing curvature;

FIG. 11 is a schematic diagram of different parking test scenarios, wherein (11a) is a parallel library location and (11b) is a vertical library location;

fig. 12 is a performance comparison box chart of the original Hybrid a (serial No. 1) and the present invention (serial No. 2) in a parallel and vertical parking scene, where (12a) is the calculation time, (12b) is the path length, and (12c) is the number of path segments;

FIG. 13 is a comparison of original Hybrid A versus the parking plan path of the present invention, wherein (13a) is a parallel library location and (13b) is a vertical library location;

fig. 14 shows the real vehicle path tracking result of scenario one in the embodiment, in which (14a) is the path tracking effect, (14b) is the steering wheel angle tracking, and (14c) is the vehicle speed tracking;

fig. 15 shows the real vehicle path tracking result of the second scenario in the embodiment, where (15a) is the path tracking effect, (15b) is the steering wheel angle tracking, and (15c) is the vehicle speed tracking;

fig. 16 shows the real vehicle path tracking result of scenario three in the embodiment, where (16a) is the path tracking effect, (16b) is the steering wheel angle tracking, and (16c) is the vehicle speed tracking;

fig. 17 shows the real vehicle path tracking result of scene four in the embodiment, where (17a) is the path tracking effect, (17b) is the steering wheel angle tracking, and (17c) is the vehicle speed tracking;

fig. 18 shows the real vehicle path tracking result of scene five in the embodiment, in which, (18a) is the path tracking effect, (18b) is the steering wheel angle tracking, and (18c) is the vehicle speed tracking;

fig. 19 is a schematic diagram of an automatic parking process according to the present invention.

Detailed Description

The invention is described in detail below with reference to the figures and specific embodiments. The present embodiment is implemented on the premise of the technical solution of the present invention, and a detailed implementation manner and a specific operation process are given, but the scope of the present invention is not limited to the following embodiments.

The parking path planning solves the problems that: the vehicle can draw an executable path connecting the poses of the targets at any initial pose with a parking space. In order to reduce tire wear, the planned path should avoid vehicle pivot steering caused by curvature discontinuity; in order to improve the parking efficiency, the length of the planned path is as short as possible, and the number of direction changes is as small as possible; in order to reduce the tracking difficulty and ensure the riding comfort, the planned path has good smoothness; in order to ensure safety, the planned path does not collide with two obstacle vehicles and a curb; in order to meet the on-line planning requirement of the real vehicle, the path planning time is controlled within 500ms as much as possible.

The library positions can be divided into three types of parallel, vertical and oblique library positions according to the library position direction. The vertical parking space can be regarded as an inclined parking space with the largest parking difficulty. Parallel and vertical library positions are selected for discussion and verification in the embodiment. The schematic diagram of the parking space scene and the parking target poses is shown in fig. 1, a rectangle filled with oblique shadows represents an obstacle car, straight lines on the upper side and the lower side represent road edges, a gray rectangle is the parking target pose, the parking target pose is equidistant from two obstacle cars, and the parking depth is the same as that of the two obstacle cars. For small parallel library positions, the target pose is often reached by performing in-library position adjustment.

The invention provides an online planning method for a continuous curvature parking path with any initial pose. The in-garage adjustment process has the characteristics of continuous and narrow parking space, the problem can be converted into the convex optimization problem of solving the curvature and the arc length of the continuous curvature arc, the solution is carried out by adopting an inner point method, and the path quality and the calculation efficiency are high. And the route planning in the warehousing process adopts an improved Hybrid A algorithm to expand state nodes in a continuous curvature form, and an executable route with continuous curvature reaching a target pose can be directly generated at any initial pose without post-processing, so that the calculation efficiency is high. As shown in fig. 2, the in-warehouse adjustment path planning is performed in two stages before the warehouse entry path planning and after the warehouse entry path tracking is completed, the former stage aims at determining the optimal warehouse entry path planning target pose, and the latter stage aims at meeting the final parking target pose requirement only by continuously performing in-warehouse adjustment path re-planning and re-tracking during in-warehouse adjustment due to the fact that the tracking length is short and the path tracking error with large curvature is large. And the path planning in the warehousing process is used for quickly calculating to obtain a collision-free path connecting any parking starting pose and the warehousing target pose.

The method comprises the following specific steps: the method comprises the steps of obtaining a position shape, a position and an initial parking pose of a garage, dividing a path planning process into a position internal adjustment path planning and a garage entering path planning, wherein the position internal adjustment path planning comprises a first position internal adjustment path planning used for obtaining a garage entering target pose and a second position internal adjustment path planning used for enabling a vehicle pose to meet the requirement of a final pose, the position internal adjustment path planning adopts an optimization method to carry out section-by-section planning, the garage entering path planning adopts an improved Hybrid A algorithm to plan to obtain a garage entering path from the current pose of the vehicle to the garage entering target pose, and finally a collision-free parking path with continuous curvature is obtained. The in-library adjustment path planning is realized by adopting an optimization method, which specifically comprises the following steps: and modeling the in-library adjustment path planning problem to solve the optimization problem of the CC circular curvature and the arc length.

1. Construction of CC circle model

1) Vehicle kinematics model

Since the vehicle speed is low (< 3km/h) during parking, the lateral dynamic effect of the tire can be ignored, and the vehicle kinematic model shown in FIG. 3 is adopted. Assuming a constant vehicle speed and a maximum vehicle speed v_maxThen, the vehicle kinematics model can be expressed in an arc length-based form, as shown in equation (1):

in the formula, x and y are coordinates (m) of the midpoint of the rear axle of the vehicle, theta is the heading angle (rad) of the vehicle, d ∈ { -1, 1} is the motion direction of the vehicle, and κ ∈ [ - κ { -C { -1 { } is the motion direction of the vehicle_max，κ_max]And σ ∈ [ σ [ [ σ ]_max，σ_max]Respectively, the path curvature (m) of the midpoint of the rear axle of the vehicle^-1) And rate of change of curvature (m)^-2) And (·)' means taking the derivative of the arc length. Maximum curvature k_maxWith maximum rate of change of curvature σ_maxLimited by the physical constraints of the actuator:

in the formula phi_maxIs the maximum front wheel angle (rad), omega_maxThe maximum front wheel rotation speed (rad/s), L the vehicle wheel base (m), and phi the front wheel rotation angle (rad).

2) Vehicle state change model

To avoid the vehicle from passing through the path tracking processIn the method, a transition curve clothoid curve with linearly changing curvature is adopted to connect two arcs or arcs and straight lines with unequal curvatures. Connecting arbitrary two curvatures k₀，κ₁∈[κ_nax，κ_max]The curvature change and the course angle change of the transition curve between the two are shown in the graphs (4a) and (4b), the shape of the corresponding clothoid curve connecting two arcs with different curvatures is shown in the graph (4c), and the curve type represented when the curvature kappa is 0 is a straight line. To improve steering flexibility and reduce path length, the invention uses a curvature change rate of σ_maxI.e. the rate of change of curvature σ ∈ { - σ { (c) } c_max，σ_max}。

Vehicle slave state [ x ]₀，y₀，θ₀，κ₀]Passing through a section with the length of L₁＝|κ₁-κ₀|/σ_maxThe curvature change rate σ ═ sign (κ)₁-κ_o)σ_maxAfter the clothoid curve of (c) becomes [ x ]₁，y₁θ₁，κ₁]The available state changes according to equation (1):

in the formula I₀＝|κ₀/σ_maxAnd | s is the path length (m). The fresnel integral operation in the equation (4) causes a large amount of calculation, and the integral operation in the equation (4) is only related to the motion direction d and the initial curvature k₀And a termination curvature k₁Therefore, the integral table for calculating each discrete curvature off-line can be adopted, and the calculation efficiency is improved by looking up the table on-line.

Vehicle from initial state [ x ]₀，y₀，θ₀，κ₀]Passing through a section with the length of L₂Arc of (a) and L₃The state change of the straight line of (2) is shown in the formulas (5) and (6), respectively.

3) Continuous curvature arc model

In order to make the path planning of the curve group and the path curvature obtained by the in-library adjustment path planning continuous, the aforementioned clothoid curve is used to connect the circular arc and the straight line to form a continuous curvature circular arc, i.e. a cc (continuous curvature) circle, the curvature change diagram is shown in fig. 5a, and the corresponding curve shape is shown in fig. 5 b. In the CC circle shown in fig. 5, the curvature changes linearly from 0 to a certain curvature κ_iMaintaining the curvature k_iOver a certain path length s_iThen, the vehicle changes linearly from the curvature back to 0, and the vehicle changes from the state [ x ]₀，y₀，θ₀，κ₀]The state after a segment of the CC circle shown in fig. 5 changes to:

in the formula, κ_iArc curvature (m) of CC circle^-1)，s_iIn the above model, the length (m) of the circular arc of the CC circle is determined when the curvature and length of the circular arc of the CC circle are determined.

2. In-store adjustment path planning

And planning the adjustment path in the reservoir by adopting the CC circular model so as to avoid the in-situ steering of the vehicle caused by the sudden change of curvature.

1) In-warehouse adjustment planning before warehouse entry path planning

And the in-warehouse adjustment path planning before the warehousing path planning adopts a method of gradually planning from a target pose to the outside by a CC circle with the minimum radius until the requirement that the vehicle can be delivered out of the warehouse without collision is met. FIG. 6 is a schematic diagram of a planning process for two in-garage adjustments of a vehicle from a target pose G (x) in an in-garage adjustment path plan prior to in-garage path planning_G，y_G，θ_G) Start to calculate in placePosture A (x)_A，y_A，θ_A) And pose B (x)_B，y_B，θ_B) In the pose B, the vehicle can be delivered from the garage without collision through the minimum turning radius CC arc taking the oout as the circle center, and the non-collision condition is the turning radius O of the front right corner point of the front wheel_outB_rrLess than the center O of the CC curve_outDistance O from left rear corner point connecting line of front obstacle vehicle_outAnd O. From this, pose B can be determined as the binned path planning target pose.

2) In-warehouse adjustment planning after warehouse-in path tracking

And adjusting in the warehouse position after the warehouse entry path is tracked, planning a target pose from the current vehicle pose, planning an adjusting path in the next section of the warehouse position of the current pose, re-planning and tracking after the planning of the adjusting path in each section of the warehouse position is finished until the vehicle pose meets the requirement of the target pose, wherein the planning process is the reverse process of the planning of the adjusting path in the warehouse position before the warehouse entry path is planned. Taking the in-storage adjustment planning process shown in fig. 7 as an example, assuming that the vehicle is located at the pose c after the in-storage path tracking is finished, the in-storage adjustment path planning calculation is performed to obtain the path CD and track the path, because of the path tracking error, the actual running path is shown as a blue dotted line, the vehicle is located at the pose D ' when the path tracking is finished, the in-storage adjustment path re-planning calculation is performed at D ' to calculate the path D ' G reaching the target pose and track the path D ', and when the vehicle pose G ' is located at the target pose G (x)_G，y_G，θ_G) And stopping the vehicle when the nearby error is within the allowable range.

Based on the CC circle model, the in-library adjustment path planning can be converted into the following convex optimization problem:

wherein the content of the first and second substances,

l_o(q_j+1)＝|θ_j+1-θ_G| (9)

f_e(d，q_j，u_j)＝q_j+f_cc(d，θ_j，u_j(1)，u_j(2)) (10)

h_P(q_j+1)＝-[d_FL，d_FR，d_RL，d_RR，d_TF，d_TR] (11)

in formulae (8) to (11), q_j＝[x_j，y_j，θ_j，κ_j]Adjusting the initial state of the path planning in the library position, wherein the state after the path planning passes through a section of CC circle is q_j+1＝[x_j+1，y_j+1，θ_j+1，κ_j+1]. Control input u_j＝[κ_i，s_i]Wherein κ is_iFor the CC circle arc to be given a curvature, s_iThe arc length is to be calculated for the CC circular arc. Optimization target l of formula (8)_oAs shown in equation (9), the difference between the vehicle heading angle and the target heading angle is shown. Constraint of equation f in equation (8)_eThe specific form is as shown in formula (10), and the state change is performed by the CC circle model shown in formula (7). The first inequality in equation (8) constrains h_pFor environmental constraints, the specific expression is formula (11), where d_FL、d_FR、d_RL、d_RR、d_TF、d_TRRespectively showing the distance from the front and rear corner points of the vehicle to the obstacle vehicles 1 and 2 and the distance from the front and rear tires close to the curb side to the curb. The second inequality in equation (8) is the control input constraint, u_min＝[-κ_max，s_min]，u_min＝[κ_max，s_max]。

Solving the convex optimization problem described by the equations (8) - (11) by using an interior point method.

3. Warehousing process path planning

The method adopts an improved Hybrid A algorithm to plan the route of the warehousing process, expands the state nodes in the form of a continuous curvature curve, plans and explores each state node to the target pose according to a pre-designed continuous curvature curve group, can directly obtain a collision-free parking route with continuous curvature, and does not need to perform post-processing to further smooth and detect collision of the route.

1) Continuous curvature curve set design

A Reeds-sheet curve (RS curve for short) formed by combining a straight line and a vehicle minimum turning radius circular arc is the shortest path connecting any two poses, and 48 RS curves can be classified into 9 curve combinations. Because the RS curve only adopts the arc with the minimum radius, the steering capacity of the vehicle under other radii is not fully exerted, the planning is not flexible enough, and the path is difficult to plan under most poses in the scene with obstacles; and the curvature of the RS curve at the joint of the straight line and the circular arc and the joint of the circular arc and the circular arc is discontinuous, so that the pivot steering of the vehicle can be caused when the path tracking is carried out.

In order to make up for the deficiency of the RS curve, the CC circle is designed into a continuous curvature curve group to avoid the in-situ steering of the vehicle; the curvature of the CC circular arc ranges from-kappa_max～κ_maxCompared with an RS curve, the steering capacity of the vehicle under different radiuses can be fully utilized, and a path planning scene is expanded. In addition, in order to simplify the calculation, the invention combines the parking experience of human drivers and 9 combination forms of RS curves, and the curve group suitable for the parallel parking scene is summarized into

And

two, the curve group suitable for the vertical parking scene is summarized as

And

three kinds of the components are adopted. Wherein L, R, s respectively represent a CC circle (κ) turning counterclockwise_i> 0), clockwise turn CC circle (kappa)_i< 0) and straight line (κ)_i0), superscript +, -indicates the direction of vehicle motion, respectively forward and reverse, subscript l_iRepresenting the length of each segment of the curve. Schematic of the above 5 combinations of continuous curvature curvesAs shown in fig. 8.

As shown in FIG. 8

The curve group is an example for explaining the path planning method of the curve group used in the present invention, and P in the figure₁For warehousing object pose, P₄Planning initial pose for curve group path, generating secondary P₁To P₄The specific solving steps of the path are as follows:

(1)P₁→P₂segment according to P₁Dot state

Selecting CC circle parameter kappa₁、l₁Obtaining P₂Dot state

(2)P₄→P₃Segment according to P₄Dot state

Selecting CC circle parameter kappa₃、l₃Obtaining P₃Dot state

(3) From P₂、P₃Point State determines the length l of the middle segment straight line₂。

The other curve group path generation methods are similar to the steps, a mode of calculation from two ends to the middle is adopted, firstly, CC circular curvature and arc length of the two ends are determined, and then, the straight line length or the CC circular curvature and the arc length of the middle section are determined according to the new state point obtained through calculation. As the CC circular curvature, the arc length of the circular arc and the length of the straight line are variable, a plurality of paths can be generated for a given starting pose and a given target pose, and the method selects the paths in the same form of path group according to the principle of the shortest path. For different types of path groups, the path is selected on the basis of the principle of minimum number of shifts, i.e. for parallel parking of two curve groupsThe priority is as follows:

the priority of three curve groups perpendicular to the oblique parking is as follows:

2) state node expansion

In order to directly generate a curvature-continuous path without post-processing to avoid additional path computation and collision detection computation, the present invention defines the jth state node as q_j＝[x_j，y_j，θ_j，κ_j]Such a four-dimensional vector expands the state nodes in a continuous curvature manner as shown in FIG. 9

The nth node obtained by the j +1 th expansion is shown. The path resulting from each state node expansion is made up of two parts, as shown in FIG. 9, including a curvature from κ_jChange to

Has a clothoid curve and a curvature of

Circular arcs or straight lines of (a).

The length of a path which passes through to reach the next state node is L after each state node expansion and fixation_eCurvature of next state node from curvature set { - κ { -_max，-κ_max+κ_step，...，κ_maxSelecting from the set, k_stepThe curvature difference between two adjacent state nodes is a constant value. Expanding to get state nodes

The length of the clothoid curve of the path is

Circular or straightThe length of the wire is L_en2＝L_e-L_en1. According to the formulas (4) to (6), the state node can be obtained

Expression (c):

in the formula, theta_tIs the heading angle (rad) after passing the clothoid curve. In addition, in order to improve the search efficiency, the nodes of the extended states in the same four-dimensional grid are combined, and only the node with the lowest evaluation function value is reserved.

3) Valuation function design

The method of the invention adopts a heuristic search algorithm in a hybrid A algorithm, measures the importance degree of the nodes in the expansion state based on the valuation function f (j), and takes the node with the minimum value f (j) as a priority expansion node in the search process. The general expression of f (j) is shown in formula (13):

f(j)＝g(j)+h(j) (13)

wherein, the cost item function g (j) is used for measuring the parking starting state node to the intermediate state node q_jThe actual cost value of; heuristic function h (j) is intermediate state node q_jCost estimate to target state node for q metric_jProximity to a target state.

In order to ensure that a planned path meets the requirements of high efficiency and comfort, namely the path length is as short as possible, the number of gear shifting is as small as possible, the curvature of the path is as small as possible, and the change direction of the curvature is prevented from being frequently changed, the invention designs g (j) shown in formulas (14) to (18):

g(j)＝g_l(j)+g_rev(j)+g_κ(j)+g_Δk(j) (14)

g_l(j)＝κ_l·Δl (15)

g_k(j)＝k_κ·|κ_j| (17)

Δκ_j＝κ_j-κ_j-1，Δκ_j-1＝κ_j-1-κ_j-2

in the above formula,. kappa_lIs the path length term coefficient; kappa_revIs a shift penalty term coefficient; kappa_κPunishment coefficient for path large curvature; kappa_Δκ1、κ_Δκ2For curvature change penalty factor coefficients, a greater penalty value is assigned to the curvature direction change, and thus κ_Δκ1＞κ_Δκ2. The value of each weight coefficient is related to the priority of each corresponding item, and each weight coefficient needs to be determined according to tests aiming at different parking scenes and parking requirements.

In order to accelerate the solving speed of the algorithm, the heuristic term shown in the formula (19) is designed, and the three contained items are respectively the shortest path heuristic term, the minimum path segment heuristic term and the obstacle avoidance heuristic term from left to right.

The obstacle avoidance heuristic item is the same as the original Hybrid A algorithm, and aims to guide the search to be far away from the U-shaped obstacle and the dead zone. Different from the original Hybrid A algorithm, the shortest path heuristic item is redesigned to adapt to the method and improve the search efficiency, the minimum path segment number heuristic item is added to make up the defect that the path result contains some sharp points for changing the path direction due to the fact that the original algorithm only considers the path length, the number of the path segments planned after the addition of the minimal path segment number heuristic item is less, and the driving experience of a human driver is better met.

The shortest path elicitation item and the minimum path segment number elicitation item are the continuous curvature path length and the path segment number under the environment that two obstacle vehicles under the minimum stock position ignore other obstacles are only considered, for the parking problem, the path planning constraint under the minimum stock position is stricter, the elicitation item value obtained under the scene is larger, and the search efficiency under other large stock position scenes can be effectively ensured. To compute the shortest path heuristic and the minimum number of path segments heuristic, assume the object pose (x)_G，y_G，θ_G) And (0, 0, 0), calculating the path from each grid center to the target pose, and performing off-line calculation because other obstacles in the environment are not considered in the calculation, and performing table lookup to obtain heuristic values only by performing simple translation and rotation calculation on line.

The size of the grid map adopted by the invention is 16m multiplied by 25m multiplied by 2 pi rad multiplied by 0.54m^-1The resolution in the x-Y direction is 0.5m, the resolution of the course angle theta is 0.1rad, and the resolution of the path curvature kappa is 0.01m^-1。

4) Collision detection

The result of the path planning is composed of straight lines, circular arcs and clothoid transition curves, areas swept by the three curves tracked by the vehicle have different contour characteristics, as shown in fig. 9, characteristic points of different contours are extracted to form external polygons of a vehicle driving area, and collision detection is carried out by checking whether a series of external polygon groups generated by the path planning intersect with the library boundary.

FIG. 10a shows a straight path, the vehicle-passing region being rectangular according to G₂、G₁And (5) obtaining the area outline by pointing the coordinates of the front corner point and the rear corner point of the vehicle. FIG. 10b shows a circular path, the inner side of which is defined by the line segment P₁P₂、P₄P₅And a segment of circular arc, and an inner broken line segment P can be used within a certain precision range₂P₃P₄Fitting an arc; the outer side of the outline is provided with a circular arc which is passed by the right rear corner point of the vehicle, a circular arc which is passed by the right front corner point of the vehicle and a line segment P connecting the two₈P₉Composition, outer broken line segment P is available within a certain precision range₆P₇P₈And P₉P₁₀P₁₁And fitting the arc. For a clothoid curve, the contour of the area where the vehicle travels when the curvature increases has a different feature from the contour of the area where the vehicle travels when the curvature decreases, and therefore the feature points should be extracted according to a different rule. Fig. 10c shows a clothoid curve path with increased curvature, and the contour features are similar to circular arcs, and feature points can be extracted in the same way. FIG. 10d shows a chothoid curve path with a decreasing curvature, and the contour of the region has a larger difference from the curve shape of FIG. 10c, which mainly runs at the right front corner of the vehicle, and is a curve with a variable concavity and convexity, so that the inflection point P of the curve is found first₇Respectively using curve segment P₆P₇And P₇P₈P₉And fitting curves on two sides.

The driving areas of the graphs (10c) and (10d) are related to the calculation of the clothoid curve, can only be obtained by a numerical calculation method, have large calculation amount, can calculate the coordinates of each characteristic point off line, and directly obtain the polygonal outline of the driving area by looking up the table on line for collision detection.

The above method, if implemented in the form of software functional units and sold or used as a stand-alone product, may be stored in a computer readable storage medium. Based on such understanding, the technical solution of the present invention may be embodied in the form of a software product, which is stored in a storage medium and includes instructions for causing a computer device (which may be a personal computer, a server, or a network device) to execute all or part of the steps of the method according to the embodiments of the present invention. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.

To evaluate the above method, the calculated time, path length, number of shifts of the inventive method were compared to the original Hybrid a. The test scenario is shown in FIG. 11, the solid points represent the X-Y coordinates of the starting pose, and the arrow direction represents the starting heading angle direction of the vehicle. 153 initial poses are tested in both parallel parking and vertical parking, the initial poses comprise 51 sites, the range of the X direction is-8 m, the range of the Y direction is 1.2-3.2 m, the interval between adjacent sites is 1m, and the heading angle of each site comprises three groups of-0.1 rad, 0 and 0.1 rad. The vehicle parameters and the test library position related parameters are shown in table 1, and the parameter meanings correspond to fig. 1 and fig. 3.

TABLE 1 vehicle parameters and test library location related parameters

Parameter(s)	Numerical value	Parameter(s)	Numerical value
				L/m	2.305	wp1/m	4.5
Lf/m	0.72	wp2/m	2
				Lr/m	0.544	dp2/m	4.85
W/m	1.551	wv1/m	3.769
				κmax/m-1	0.27	wv2/m	4.5
σmax/m-2	0.4	dv2/m	2.251

According to the parameters in the table 1, the final parking target pose of the parallel parking space is (x)_pG，y_pG，θ_pG) And (x) the final parking target pose of the vertical parking position is (-3.6655, -0.7755, 0)_vG，y_vG，θ_vG) (-1.1255, -3.025, pi/2). Because the parallel position of the warehouse is smaller and the target pose can be reached through an in-warehouse adjustment party, the in-warehouse adjustment initial pose (x) in the warehouse is obtained by solving the in-warehouse adjustment planning before the warehouse entry path planning_B，y_B，θ_B) (-4.0891, -0.945, 0.3016) is used as a target pose for warehousing process path planning, and then the original Hybrid a-algorithm is compared with the simulation of the method of the present invention, and the comparison result is shown in fig. 12 and 13. The algorithm operating platform is dSPACE AutoBox II 1401/1513, and the dominant frequency of the processor is 900 MHz.

The distribution of values, including mean, optimum and worst values, is visualized by the box plot of FIG. 12.

According to the graph (12a), the calculation time of the method is less than that of the original Hybrid A algorithm, the solving efficiency is high, the performance is stable, the calculation time under all the test poses is less than 500ms, and the online planning requirement can be met. Furthermore, it can be seen from fig. 12 that vertical parking takes longer than parallel parking path planning calculations because: 1) the parallel parking expansion curve groups are only two, the vertical parking expansion curve groups are three, and the calculation time is longer when the nodes expand the state of the curve groups; 2) the road width of 4.5m is narrower for vertical parking, the number of searched state nodes in a vertical parking scene is more, and the calculation time is longer.

According to the graphs (12b) and (12c), the path length original Hybrid A algorithm obtained by the method has little difference, and is superior to the original Hybrid A algorithm in some scenes, and the number of path segments is less than that of the original Hybrid A algorithm. Theoretically, the method considers the constraint of the path curvature change rate, and adds a path large curvature penalty term and a curvature change penalty term in the evaluation function for taking comfort into account, so that the path length is slightly longer than the original Hybrid A algorithm in most scenes, but the path length and the number of path segments are both smaller than the original Hybrid A algorithm in scenes similar to those in FIG. 13.

In the initial pose 1 of fig. 13, the number of path segments of both methods is 1 segment, but the path length of the method of the present invention is smaller than that of the original Hybrid a, because the Reed-Shepp curve group adopted by the curve group extension of the original Hybrid a is composed of an arc with the minimum turning radius and a straight line, and the curve group adopted by the method of the present invention includes a series of different turning radius values, although the length of the curve segment is larger than that of the RS curve group, the length of the straight line segment is much smaller than that of the RS curve group.

In fig. 13, under the initial pose 2 and the initial pose 3, the RS curve group cannot calculate and obtain a collision-free path of the warehousing target pose, the original Hybrid a can obtain a solution by multiple state node expansion, and the curve group designed by the method of the present invention can be directly planned to obtain a shorter path length.

In the initial pose 4 of fig. 13, the path segment planned by the method of the present invention is 1 less than the original Hybrid a, because the design of the shift penalty term in the cost term function and the shift frequency estimation term in the heuristic term function of the present invention effectively makes the search converge to the state point with less shift frequency, and finally obtains the planning result with less path segment number.

In order to verify the real vehicle traceability of the planned route, the present embodiment performs a real vehicle test on an intelligent driving test platform modified from a pure electric vehicle based on nabobism E50. The vehicle is equipped with an ultrasonic radar, a single line laser radar, a multi-line laser radar, a monocular camera, a look-around camera, a GPS/INS combined navigator RT3000 and a MicroAutoBox of dSPACE as a controller to run automatic driving decision, planning and control algorithms.

Compiling and writing a path planning model established in an MATLAB/Simulink environment into a dSpace MicroAutobox controller, planning a path in real time on line, and tracking the path by adopting a controller designed by the existing literature.

In this embodiment, a real-time vehicle test is performed in five scenes, where a scene one is one-segment path parallel parking, a scene two is two-segment path parallel parking, a scene one is one-segment path vertical parking, a scene one is two-segment path vertical parking, and a scene one is three-segment path vertical parking, and the planning and tracking result is shown in fig. 14 to 18.

According to fig. 14-18, the planned path can be well tracked by the vehicle without collision, and the steering wheel angle change is smoother and has no original steering during parking. The X, Y direction deviation and the heading angle deviation of the final parking pose and the target parking pose in the five scenes in fig. 14-18 are shown in table 2, the X direction deviation is within 0.07m, the Y direction deviation is within 0.08m, the heading angle deviation is within 0.8deg, and the final parking pose has higher precision.

TABLE 2 deviation between final parking pose and target pose of real vehicle

Scene	X direction/m	Y direction/m	Course angle/deg
				Scene one	-0.0670	-0.0758	-0.5598
Scene two	-0.0219	-0.0201	-0.3390
				Scene three	-0.0060	-0.0625	-0.0373
Scene four	0.0357	0.0180	0.7236
				Scene five	0.0112	-0.0197	0.1857

The invention provides a sectional type path planning method capable of planning a continuous curvature path at any initial pose. Simulation results show that the method can plan the parking path at any initial pose, the solving time is less than 500ms, and the method has online planning capability; compared with the original Hybrid A algorithm, the improved Hybrid A algorithm planning path adopted by the invention has higher calculation efficiency, shorter planning path length, fewer path segments and higher parking efficiency. The actual vehicle test result shows that the planned path can be well tracked by the vehicle and has no original steering, the horizontal and longitudinal tracking deviation is within 8cm, and the course angle deviation is within 0.8 deg.

In another embodiment, there is provided a continuous curvature automatic parking system in an arbitrary initial pose, the system comprising: the path online planning module is used for obtaining a parking path by adopting the online planning method; and the tracking module executes parking based on the parking path, tracks the parking track, judges whether collision risks exist or whether a global obstacle map is updated in real time, and calls the path online planning module to perform re-planning until parking and warehousing if the collision risks exist or the global obstacle map is updated, and if the collision risks exist or the global obstacle map is updated, the path online planning module is called to perform re-planning.

As shown in fig. 19, the parking process of the parking system in this embodiment includes the steps of:

1) detecting a library position;

2) judging whether the library position meets the berthable requirement, if so, executing a step 3), otherwise, executing a step 1);

3) acquiring the shape and position of a parking space and the initial pose of parking;

4) planning a warehousing path: firstly, adjusting and planning in a warehouse location before planning a warehouse entry path, and calculating a warehouse entry target pose according to a warehouse location shape; planning a warehousing path from the current pose of the vehicle to the target pose based on an improved Hybrid A algorithm;

5) tracking the planned warehousing path;

6) tracking and judging whether the vehicle has collision risk, if so, stopping tracking and executing the step 4), otherwise, executing the step 7);

7) judging whether the global obstacle map is updated or not while tracking (whether the library position is changed), if so, stopping tracking and executing the step 4), and if not, executing the step 8);

8) judging whether the warehousing path tracking is finished or not, if so, executing a step 9), and otherwise, executing a step 5);

9) judging whether the pose of the finished warehousing path tracking is consistent with the pose of the warehousing target, if so, executing the step 10), and otherwise, executing the step 4);

10) judging whether the in-garage adjustment is needed, if so, executing the step 11), and if not, ending the parking;

11) performing in-library adjustment path planning based on an optimization method;

12) tracking an in-store adjustment path;

13) and judging whether the vehicle pose meets the final pose requirement, if so, ending parking, and otherwise, executing the step 11).

The foregoing detailed description of the preferred embodiments of the invention has been presented. It should be understood that numerous modifications and variations could be devised by those skilled in the art in light of the present teachings without departing from the inventive concepts. Therefore, the technical solutions available to those skilled in the art through logic analysis, reasoning and limited experiments based on the prior art according to the concept of the present invention should be within the scope of protection defined by the claims.

Claims (10)

1. The method is characterized in that a parking path is obtained by adopting a sectional path planning idea, the path planning process is divided into a warehouse location internal adjustment path planning and a warehouse entry path planning, the warehouse location internal adjustment path planning comprises a first warehouse location internal adjustment path planning used for obtaining a warehouse entry target pose and a second warehouse location internal adjustment path planning used for enabling the vehicle pose to meet the final pose requirement, the warehouse location internal adjustment path planning adopts an optimization method to carry out section-by-section planning, and the warehouse entry path planning adopts an improved hybrid A algorithm planning to obtain a warehouse entry path from the current pose of a vehicle to the warehouse entry target pose.

2. The method for the on-line planning of the continuous curvature parking path with any initial pose according to claim 1, wherein the optimization method is adopted to realize the in-garage adjustment path planning and specifically comprises the following steps:

modeling the in-library adjustment path planning problem to solve the optimization problem of the CC circular curvature and the arc length, wherein the constructed optimization model is expressed as follows:

s.t.q_j+1＝f_e(d，q_j，u_j)

h_p(q_j+1)≤0

u_min≤u_j≤u_max

wherein the objective function l_o(q_j+1)＝|θ_j+1-θ_GI is the vehicle heading angle theta_j+1Angle theta with target course_GDifference of (q)_j+1Adjusting the state q in path planning for in-bin_jState after passing through a section of CC circle, u_jFor control input, u_minAnd u_minFor controlling the minimum and maximum values of the input, the constraint h_pRepresenting environmental constraints, and d ∈ { -1, 1} is the vehicle motion direction.

3. The method for online planning of a continuous curvature parking path with any initial pose according to claim 2, wherein the CC circle is a continuous curvature arc formed by connecting a clothoid curve, an arc and a straight line.

4. The method for the on-line planning of the continuous curvature parking paths with any initial poses according to claim 2, wherein the planning of the adjustment paths in the first garage position adopts a method of gradually planning from the final poses to the outside by a CC circle with the minimum radius until the requirement that the vehicles can leave the garage without collision is met, and the poses of the garage targets are obtained.

5. The method for the on-line planning of the continuous curvature parking paths with any initial pose according to claim 2, wherein the second storehouse position inner adjustment path planning adopts a method of gradually planning outward by a CC circle with a minimum radius from the current pose until the requirement that the vehicle can be delivered from the storehouse without collision is met, the adjustment path in the next section of the storehouse position of the current pose is obtained, and re-planning and tracking are performed after the planning of the adjustment path in each section of the storehouse position is finished until the vehicle pose meets the requirement of the final pose.

6. The method for the on-line planning of the continuous curvature parking path with any initial pose according to claim 1, wherein the step of obtaining the parking path from the current pose of the vehicle to the parking target pose by using the improved Hybrid a algorithm comprises the following specific steps:

designing a continuous curvature curve group based on a CC circle, selecting a path in the same form curve group according to a shortest path principle, and selecting a path according to a principle of minimum gear shifting times for different forms of curve groups;

and expanding the state nodes in a continuous curvature mode based on the continuous curvature curve group, measuring the importance degree of the expanded state nodes based on the designed evaluation function, and taking the node with the minimum evaluation function value as a priority expansion node in the searching process to obtain a final warehousing path.

7. The method for online planning of a continuous curvature parking path with any initial pose according to claim 6, wherein the valuation function f (j) is expressed as:

f(j)＝g(j)+h(j)

wherein, the cost item function g (j) is used for measuring the parking starting state node to the intermediate state node q_jThe heuristic function h (j) is an intermediate state node q_jCost estimate to target state node for q metric_jProximity to a target state.

8. The method for online planning of a continuous curvature parking path with any initial pose according to claim 7, wherein the cost term function g (j) is expressed as:

g(j)＝g_l(j)+g_rev(j)+g_κ(j)+g_Δκ(j)

g_i(j)＝k_l·Δl

g_k(j)＝k_κ·|κ_j|

Δκ_j＝κ_j-κ_j-1，Δκ_j-1＝κ_j-1-κ_j-2

wherein the cost term function g (j) is composed of a path length term g_l(j) Shift penalty term g_rev(j) Large curvature penalty term g_κ(j) And a curvature change penalty term g_Δκ(j) Composition k_lIs the path length term coefficient; k is a radical of_revIs a shift penalty term coefficient; k is a radical of_κPunishment coefficient for path large curvature; k is a radical of_Δκ1、k_Δκ2Penalizing the coefficient of the term for curvature change, and k_Δκ1＞k_Δκ2；κ_jIs the curvature,. DELTA.l is the change in arc length, d_jFor the direction of movement of the vehicle, Δ κ_jIs a change in curvature.

9. The method for online planning of a continuous curvature parking path with any initial pose according to claim 7, wherein the heuristic function h (j) is expressed as:

h(j)＝k_nonholo·h_nonholo(j)+k_shift·h_shift(j)+k_holo·h_holo(j)

wherein the heuristic function h (j) is derived from the shortest path heuristic h_nonholo(j) Minimum path segment number heuristic h_shift(j) And obstacle avoidance elicitor h_holo(j) Composition of h_nonholo(j) In order to take into account only environmental obstacles and not the path length from each grid point to the target point when the vehicle is not constrained by integrity, h_nonholo(j) And h_shift(j) When the environmental barrier is not considered, the length of the continuous curvature path from each grid point to the target pose and the number of the path segments are calculated by the continuous curvature curve group.

10. A continuous curvature automated parking system in an arbitrary initial pose, the system comprising:

a path online planning module, which adopts the online planning method as claimed in claim 1 to obtain a parking path;

and the tracking module executes parking based on the parking path, tracks the parking track, judges whether collision risks exist or whether a global obstacle map is updated in real time, and calls the path online planning module to perform re-planning until parking and warehousing if the collision risks exist or the global obstacle map is updated, and if the collision risks exist or the global obstacle map is updated, the path online planning module is called to perform re-planning.

Images