CN115456297A - Automatic parking path optimization method and device - Google Patents
Automatic parking path optimization method and device Download PDFInfo
- Publication number
- CN115456297A CN115456297A CN202211190937.0A CN202211190937A CN115456297A CN 115456297 A CN115456297 A CN 115456297A CN 202211190937 A CN202211190937 A CN 202211190937A CN 115456297 A CN115456297 A CN 115456297A
- Authority
- CN
- China
- Prior art keywords
- path
- optimization
- initial
- ref
- track
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000005457 optimization Methods 0.000 title claims abstract description 131
- 238000000034 method Methods 0.000 title claims abstract description 46
- 230000004888 barrier function Effects 0.000 claims abstract description 7
- 230000000452 restraining effect Effects 0.000 claims description 7
- 238000004590 computer program Methods 0.000 claims description 6
- 230000006870 function Effects 0.000 claims description 4
- 239000011159 matrix material Substances 0.000 claims description 4
- 238000010276 construction Methods 0.000 claims description 2
- 238000001514 detection method Methods 0.000 description 4
- 230000008859 change Effects 0.000 description 2
- 238000009499 grossing Methods 0.000 description 2
- 230000009467 reduction Effects 0.000 description 2
- 230000007704 transition Effects 0.000 description 2
- 150000001875 compounds Chemical class 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000035772 mutation Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
- G06Q10/00—Administration; Management
- G06Q10/04—Forecasting or optimisation specially adapted for administrative or management purposes, e.g. linear programming or "cutting stock problem"
- G06Q10/047—Optimisation of routes or paths, e.g. travelling salesman problem
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Abstract
The invention discloses an automatic parking path optimization method and device, wherein the method comprises the following steps: obtaining barrier information, generating an initial path planning track, and converting each path of the initial path planning track into a plurality of discrete initial track points P i ref (x i ref ,y i ref ) (ii) a Planning each path point coordinate P of each path in the path by the path i (x i ，y i ) For optimizing variables, constructing a sequence quadratic programming problem by taking the maximum curvature limitation, fixed initial position and destination vehicle position and non-collision of each track point with an obstacle as optimization constraint conditions and taking the smoothness of a path, the path length and the distance of the deviation from the initial track point as optimization targets, and performing iterative solution to generate track point information; generating an optimized planning path according to the track point information; the numerical value of the optimization target is the sum of the weights of the optimization targets, and in the path smoothness, the weights of the starting coordinate and the destination coordinate are both larger than other tracks between the starting coordinate and the destination coordinateWeight of the locus coordinates.
Description
Technical Field
The invention relates to the field of automatic driving, in particular to path optimization of automatic parking.
Background
After the automatic parking path planning, the path needs to be smoothed so that the curvature has no abrupt change. At present, curve transition methods and optimization methods are mainly used, curve transition generally only carries out smooth processing on curvature convex changing positions, a gradient-free optimization algorithm cannot guarantee solving efficiency, and most of the existing smooth algorithms may have the risks that a local track has large deviation with a target point and is excessively smooth, so that a vehicle and an obstacle have collision. Because the curvature constraint of the path is nonlinear, the curvature constraint cannot be added in the quadratic programming, and the existing optimized planned path often has the problems of sudden change of curvature and collision between a vehicle and an obstacle caused by excessive smoothness of the vehicle.
Therefore, in patent publication CN112277932a, an automatic driving parking path planning method is disclosed, which introduces path curvature constraint into constraint conditions and optimization targets of an optimization function, so that the optimized parking trajectory and curvature are continuous, and the steering wheel changes continuously when the vehicle travels along the trajectory, and does not need to stop. However, the optimization method only optimizes a few points of the initial path, cannot ensure that the whole path meets the constraint condition and is limited to the parallel parking planning problem that the initial position of the vehicle limits a certain range, and meanwhile, the optimization method (geometric method) in the patent can only be used for optimizing the path planning method, has no universality and is difficult to use in the actual parking path planning, and during the optimization, the problem of greatly driving a steering wheel when the vehicle is static cannot be solved.
Therefore, there is a need for an automatic parking path planning method and apparatus that can solve the above problems.
Disclosure of Invention
The invention aims to provide an automatic parking path optimization method and device, which can effectively reduce the risk of collision between a vehicle and an obstacle caused by the fact that an optimized planned path is excessively smooth.
In order to achieve the purpose, the invention discloses an automatic parking path optimization method, which comprises the following steps: obtaining barrier information, generating an initial path planning track, and converting each path of the initial path planning track into a plurality of discrete initial track points P i ref (x i ref ,y i ref ) I =1, 2, 3 … n; planning the coordinate P of each track point in each path of the track by the path i (x i ，y i ) For optimizing variables, using the maximum curvature limitation, the fixed starting place and the pose of the destination vehicle and the non-collision of each track point with the barrier as optimization constraint conditions, using the smoothness of the path, the path length and the distance from the initial track point as optimization targets, constructing a sequence quadratic programming problem, and inquiring the sequence quadratic programmingPerforming iterative solution on the questions to generate track point information; generating an optimized planning path according to the track point information; wherein the numerical value of the optimization target is the sum of the path smoothness, the path length and the weight of the distance deviating from the initial track point, and in the path smoothness, the initial coordinate P 1 (x 1 ，y 1 ) And destination coordinates P n (x n ，y n ) Are all greater than the track point coordinate P between the starting place coordinate and the destination coordinate 2 (x 2 ，y 2 ) To P n-1 (x n-1 ，y n-1 ) The weight of (c).
Preferably, the optimization constraints that limit the maximum curvature include:
wherein the vector
C max For curvature limits, Δ l is the mean of the spacing between adjacent trace points.
Preferably, when the iterative solution of the sequential quadratic programming problem is performed, the optimization constraint function that limits the maximum curvature is linearly expanded as:
Preferably, the constraint condition that each track point does not collide with the obstacle is as follows: origin coordinate P 1 (x 1 ，y 1 ) And destination coordinates P n (x n ，y n ) Invariant, track point coordinates P 2 (x 2 ，y 2 ) To P n-1 (x n-1 ，y n-1 ) With the initial locus point P i ref (x i ref ,y i ref ) The deviation value between the X coordinate and the Y coordinate is less than or equal to
And di is the distance between the corresponding vehicle and the obstacle at each initial track point in each path of the path planning track.
Preferably, the heading angle constraint condition is:
Preferably, the path smoothness cost is:
the path length penalty is:
the cost of the distance from the initial track point is:
P smooth 、P length and P ref Are penalty factors of smoothness, length and distance relative to the initial track point respectively,
is a track point P i The weight of the smoothness is such that,
middle is a track point P i Weight of distance deviating from the initial track point;
the optimization target is as follows:
h is a semi-positive definite symmetric matrix, g is a gradient vector, x = { x = 0 ,y 0 ,x 1 ,y 1 ,…，x n+1 ,y n+1 } T 。
Preferably, the iterative solution of the sequential quadratic programming problem specifically includes: giving a motion limit of the optimized variable, performing an optimized approximate problem structure, performing sequential quadratic programming solution on the optimized approximate problem to obtain a new optimized variable, updating the optimized variable according to the new optimized variable, calculating a numerical value of an optimized target and a constraint value of an optimized constraint condition, judging whether the optimized constraint condition is qualified or not according to the numerical value of the optimized target, outputting the optimized variable when the optimized constraint condition is qualified and the optimized constraint condition is converged, performing next iterative sequential quadratic programming problem solution when the optimized constraint condition is not qualified, reducing the motion limit and performing next iterative sequential quadratic programming problem solution when the optimized constraint condition is not qualified.
Specifically, the reduced motion limit is specifically: and reducing the preset multiplying power of the optimized constraint condition of the violation constraint.
Preferably, when judging whether the optimization constraint condition is qualified or not according to the constraint value of the optimization constraint condition, judging whether the optimization variables are all at the edge of the motion interval of the optimization variables in multiple iterations, and if so, enlarging the motion limit to increase so as to improve the solving efficiency.
Specifically, the extended motion limits are specifically: and expanding the preset multiplying power when the optimization variable is positioned at the edge in the multi-iteration optimization.
Specifically, when judging whether the optimization target and the optimization constraint condition are qualified, if the maximum curvature condition is violated, the weight of the path smoothness cost is added to the optimization target.
Specifically, a Hybrid A algorithm is adopted to perform path planning to generate the initial path planning track, and each segment of the initial path planning track is decomposed into n +2 discrete initial track points P i ref (x i ref ,y i ref )，i＝0、1、2、3…n、n+1，P 0 ref (x 0 ref ,y 0 ref ) Auxiliary points P added for restraining the initial ground course angle of the vehicle in each path of the path planning track in a constant way n+1 ref (x n+1 ref ,y n+1 ref ) And auxiliary points which are added for restraining the constant vehicle destination course angle in each path of the path planning track. The Hybrid A algorithm is used for initial path planning, and the method is suitable for parking in any parking space and has wide applicability. Of course, the invention can also adopt other path planning methods to obtain the initial path planning track so as to carry out subsequent path smoothing processing.
The invention also discloses an automatic parking path optimizing device, which comprises: the detection module acquires obstacle information; an initial path planning module for generating an initial path planning track and converting each path of the initial path planning track into a plurality of initial track points P i ref (x i ref ,y i ref ) (ii) a A problem component module for planning the coordinate P of each track point in each path of the track by the path i (x i ，y i ) For optimizing variables, constructing a sequence quadratic programming problem by taking the maximum curvature limitation, the fixed initial position and the fixed destination vehicle position and the non-collision of each track point with an obstacle as optimization constraint conditions and taking the smoothness of a path, the path length and the distance of deviating from the initial track point as optimization targets, wherein i =1, 2 and 3 … n; problem solving Module, for sequence twoIteratively solving the secondary planning problem to generate track point information; the path optimization module generates an optimized planning path according to the track point information; wherein the numerical value of the optimization target is the sum of the path smoothness, the path length and the weight deviating from the distance of the initial track point, and in the path smoothness, the initial position coordinate P 1 (x 1 ，y 1 ) And destination coordinates P n (x n ，y n ) Are all greater than the track point coordinate P between the starting place coordinate and the destination coordinate 2 (x 2 ，y 2 ) To P n-1 (x n-1 ，y n-1 ) The weight of (c).
The invention also discloses an automatic parking path optimizing device, which comprises: one or more processors; a memory; and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by one or more processors to implement the automatic parking path optimization method as described above.
The present invention also discloses a computer-readable storage medium comprising a computer program for use in conjunction with an electronic device having a memory, the computer program being executable by a processor to implement the automatic parking path optimization method as described above.
Compared with the prior art, the method and the device have the advantages that curvature constraint is used as one of the sequential quadratic programming conditions during path optimization, the optimized target is set as the weight sum of multiple targets such as path smoothness, the optimized path is more suitable for different environments, the weight of the path smoothness is set to be larger at the starting place and the destination, and the risk that the vehicle collides with an obstacle due to the fact that the optimized planned path is excessively smooth is effectively reduced. Moreover, the method and the device perform discrete processing on each section of the initial path planning track to obtain discrete points of all path planning tracks as track points so as to perform all-around optimization on the whole path planning track and ensure that the optimized whole line meets the constraint condition, and meanwhile, the method and the device are suitable for path smoothing processing after the initial planning tracks are obtained by multiple path planning methods, and have strong applicability.
Drawings
Fig. 1 is a flowchart of an automatic parking route optimization method according to the present invention.
Fig. 2 is a graph of a section of a path in an initial path planning trajectory in accordance with the present invention.
Fig. 3 is a graph of a section of a path in a path planning trajectory obtained by sequential quadratic programming solution according to the present invention.
FIG. 4 is a flow chart of the sequential quadratic programming solution of the present invention.
Fig. 5 is a structural diagram of the automatic parking path optimizing apparatus of the present invention.
Detailed Description
In order to explain technical contents, structural features, and objects and effects of the present invention in detail, the following detailed description is given with reference to the accompanying drawings in conjunction with the embodiments.
Referring to fig. 1, the present invention discloses an automatic parking path optimizing method including steps S11 to S14.
S11, obtaining barrier information, generating an initial path planning track, and converting each path of the initial path planning track into a plurality of discrete initial track points P i ref (x i ref ,y i ref ). i =1, 2, 3 … n. The specific method for acquiring the obstacle information may be obtained through detection of a related sensor, may be input through an external device, or may be directly acquired, for example, according to map information.
The generated initial path planning track may have one section or multiple sections, and specifically, the whole initial path planning track is divided into corresponding sections of paths according to the driving direction.
Wherein, the invention converts each path of the initial path planning path into a plurality of discrete initial path points P i ref (x i ref ,y i ref ). The initial track points are provided with a plurality of discrete points on the whole path of the path planning track. The number of the track points in each path can be equal or unequal, and the specific number of the track points is determined by the distance range of the adjacent track points or the actual complex situation of the pathAnd (4) determining.
Referring to fig. 2, the present invention performs path planning by using Hybrid a-x algorithm to generate the initial path planning trajectory, and decomposes each segment of the initial path planning trajectory into n +2 discrete initial trajectory points P i ref (x i ref ,y i ref )，i＝0、1、2、3…n、n+1。
The invention combines the Hybrid A algorithm with the sequence quadratic programming algorithm, carries out omnibearing optimization on the path planning track obtained by the Hybrid A algorithm, can ensure that the optimized whole line conforms to the constraint condition, is suitable for parking in any parking space, and has wide applicability. Wherein, the initial track point P 0 ref (x 0 ref ,y 0 ref ) Auxiliary points, P, added for each path of the initial path planning trajectory, which constrain the unchanged initial course angle of the vehicle 0 ref (x 0 ref ,y 0 ref ) And P 2 ref (x 2 ref ,y 2 ref ) The slope of the connecting line is equal to the slope corresponding to the initial course angle of the vehicle, P 0 ref (x 0 ref ,y 0 ref ) And P 1 ref (x 1 ref ,y 1 ref ) Pitch and P of 1 ref (x 1 ref ,y 1 ref ) And P 2 ref (x 2 ref ,y 2 ref ) Are equally spaced.
Wherein, the initial track point P n+1 ref (x n+1 ref ,y n+1 ref ) For each path of the initial path planning trajectory, constraining the vehicle 200 to an auxiliary point, P, added with a constant destination course angle n-1 ref (x n-1 ref ,y n-1 ref ) And P n+1 ref (x n+1 ref ,y n+1 ref ) The slope of the connection line is equal to the slope corresponding to the course angle of the vehicle destination, P n+1 ref (x n+1 ref ,y n+1 ref ) And P n ref (x n ref ,y n ref ) Pitch and P of 1 ref (x 1 ref ,y 1 ref ) And P 2 ref (x 2 ref ,y 2 ref ) Are equally spaced.
S12, planning the coordinate P of each track point in each path of the track by using the path i (x i ，y i ) For optimizing variables, the maximum curvature is limited, the initial position and the position of a destination vehicle are fixed (the initial position and a destination coordinate point are fixed, and a course angle is fixed), each track point does not collide with an obstacle as an optimization constraint condition, and the smoothness, the path length and the distance deviating from the initial track point (the deviation value of the coordinate of the track point in each section of the path planning path and the coordinate of the initial track point in the corresponding section of the path planning path) of the path are taken as optimization targets to construct a sequence secondary planning problem. Wherein i =0, 1, 2, 3 … n, n +1.
Referring to fig. 3, each path segment of the initial path planning trajectory is optimized through a sequential quadratic programming algorithm to obtain path trace points of a segment corresponding to the optimized path planning trajectory. In each section of the path planning track, the coordinates of the vehicle starting place are P 1 (x 1 ，y 1 ) With vehicle destination coordinates of P n (x n ，y n ) Point of track P 0 (x 0 ，y 0 ) Auxiliary points, track points P, added for constraining the initial course angle of the vehicle to be constant n+1 (x n+1 ，y n+1 ) And auxiliary points are added for restraining the destination heading angle of the vehicle from being unchanged.
Wherein the numerical value of the optimization target is the sum of the path smoothness, the path length and the weight of the distance deviating from the initial track point. The weights of the different optimization objectives (including path smoothness, path length, and distance from the initial trajectory point) may be adjusted according to the actual parking environment, and are not fixed values, such as the distance d between the obstacle and the obstacle corresponding to the distance between the vehicle 200 and the obstacle at each initial trajectory point i Is less than a predetermined value, on the basis of an initial weightAnd increasing the weight of the distance deviating from the initial track point and/or reducing the weight of the path length.
Step S12 also includes calculating the distance d according to the distance between the initial track point and the obstacle i . Wherein, the distances between the four line segments of the vehicle 200 and the obstacle line segment are solved, and the minimum value is taken as the shortest distance d between the vehicle 200 and the obstacle i 。
In the same optimization target, the weights of different track points can be the same or different and are set according to actual requirements. For example, in the present embodiment, in the path smoothness, the start position coordinate P 1 (x 1 ，y 1 ) And destination coordinates P n (x n ，y n ) Are all greater than the coordinate P of the track point between the starting location coordinate and the destination coordinate 2 (x 2 ，y 2 ) To P n-1 (x n-1 ，y n-1 ) The weight of (c).
In the path smoothness of the embodiment, the coordinate P of the track point 2 (x 2 ，y 2 ) To P n-1 (x n-1 ，y n-1 ) The weights between the points can be the same, or the weights of the first m track points in the smoothness of the path can be gradually reduced, and the weights of the last m track points can be gradually increased, for example, the weight of the track point P is gradually increased 1 (x 1 ，y 1 ) To point of track P 3 (x 3 ，y 3 ) Gradually decrease in weight of the point of trace P n-2 (x n-2 ，y n-2 ) To the track point P n (x n ，y n ) M is an integer of 1 or more, and may be 1, 2, 3, etc., and the value thereof is n/2 or less.
And S13, carrying out iterative solution on the sequence quadratic programming problem to generate track point information.
S14, according to the track points P in the track point information in all the section paths 1 (x 1 ，y 1 ) To P n (x n ，y n ) And generating an optimized planning path.
When the initial path planning track has multiple sections of paths, in step S14, the path points P in the paths of all the sections are determined 1 (x 1 ，y 1 ) To P n (x n ，y n ) Generating a final optimized total planning path, wherein the track points P in the path can be directly planned according to the initial paths of all the segments 1 (x 1 ，y 1 ) To P n (x n ，y n ) Generating the optimized planning path, or planning the trajectory point P in the trajectory according to the initial path of each segment 1 (x 1 ，y 1 ) To P n (x n ，y n ) And generating each optimized planning path, and combining all the optimized planning paths into an overall planning path.
In step S12, the specific content and formula of the optimization constraint condition are as follows:
the optimization constraints limiting the maximum curvature are:
wherein the vector
C max Δ l is the mean of the distances between adjacent trace points, which is the curvature limit. Delta l is obtained by solving the last step of optimization iteration
Middle and track point P 1 (x 1 ,y 1 ) To P n (x n ,y n ) The mean value of the distance between adjacent track points. When the optimization iteration is performed for the first generation, the delta l is specifically the initial track point P 1 ref (x 1 ref ,y 1 ref ) To P n ref (x n ref ,y n ref ) The mean value of the distance between adjacent track points.
The constraint condition that each track point does not collide with the barrier is as follows: origin coordinate P 1 (x 1 ，y 1 ) And destination coordinates P n (x n ，y n ) Is not changedCoordinate of point of track P 2 (x 2 ，y 2 ) To P n-1 (x n-1 ，y n-1 ) With the initial locus point P i ref (x i ref ,y i ref ) The deviation value between the X coordinate and the Y coordinate is less than or equal to
d i And planning the distance between the corresponding vehicle and the obstacle at each track point in each section of the path. Coordinate of point of track P 0 (x 0 ，y 0 ) To the initial track point coordinate P 0 ref (x 0 ref ,y 0 ref ) Between X coordinate and Y coordinate is less than or equal to d c ，d c Setting an upper limit value for a predetermined coordinate position offset, d c Is greater than or equal to
Specifically, the constraint formula is:
in the formula (I), the compound is shown in the specification,
wherein:
the course angle constraint conditions are as follows:
In step S12, the specific content and formula of the optimization target are as follows:
the path smoothness is: (x) i-1 +x i+1 -2x i ) 2 +(y i-1 +y i+1 -2y i ) 2 。
The path smoothness cost is:
the path length is: (x) i+1 -x i ) 2 +(y i+1 -y i ) 2 。
The path length penalty is:
the distance from the initial track point is as follows:
the cost of the distance from the initial track point is:
P smooth 、P length and P ref Penalty factors, which are smoothness, length and distance from the initial trace point, are preset constants.
Is a track point P i The weight of the smoothness is such that,
middle is a track point P i And the weight of the distance deviating from the initial track point is a numerical value set during the quadratic programming of the sequence, and the initial value is a preset value. In this embodiment, each of the optimized target path smoothness, path length, and departure from the initial trajectoryThe weight ratio of the locus distance is:
of course, the weight of each optimization objective is limited in the above formula, and the weight of the path length may also be a set value instead of being fixed on the number 1.
The optimization target is as follows:
h is a (2n + 4) x (2n + 4) semi-positive definite symmetric matrix, and g is a 2n +4 dimensional gradient vector, which is a constant calculated by the above formula. x = { x 0 ,y 0 ,x 1 ,y 1 ,…，x n+1 ,y n+1 } T Is a coordinate P of a track point 0 -P n+1 The inversion of the matrix is a variant of the sequence quadratic programming.
In step S12, an optimization problem can be established according to the constraint conditions and the optimization objective:
find x＝{x 0 ,y 0 ,x 1 ,y 1 ,…,x n+1 ,y n+1 } T
minimize
subject to(x i-1 +x i+1 -2x i ) 2 +(y i-1 +y i+1 -2y i ) 2 ≤Δl 4 (C max ) 2 (i＝1,2,…,n)
y 2 -y 0 ＝(x 2 -x 0 )tan(yaw 1 )
y n+1 -y n-1 ＝(x n+1 -x n-1 )tan(yaw n )
in step S13, when the iterative solution of the sequential quadratic programming problem is performed,
the optimization constraint function that limits the maximum curvature is linearly expanded as:
In step S13, the iterative solution of the sequential quadratic programming problem specifically includes steps S31 to S39:
s31 given an optimization variable P i (x i ，y i ) Initial limit of motion
In this step, other parameters (including yaw) are also performed 1 、yaw 2 Parameters such as Cmax) are initialized.
S32 performs optimization approximation problem construction (QP). The method specifically comprises the following steps:
find x＝{x 0 ,y 0 ,x 1 ,y 1 ,…,x n+1 ,y n+1 } T
minimize
subject to
y 2 -y 0 ＝(x 2 -x 0 )tan(yaw 1 )
y n+1 -y n-1 ＝(x n+1 -x n-1 )tan(yaw n )
S33, carrying out sequential quadratic programming solution on the optimization approximation problem to obtain a new optimization variable.
And S34, updating the optimization variables according to the new optimization variables.
S35, calculating the value of the optimization target and the constraint value of the optimization constraint condition.
And S36, judging whether convergence occurs according to the numerical value of the optimization target. And if the difference value between the optimized target value and the previous iterative optimization target value is greater than the preset value or the optimized target value is greater than the previous iterative optimization target value, judging that the optimized target value is not converged.
And S37, judging whether the optimization constraint condition is qualified or not according to the constraint value of the optimization constraint condition. Specifically, the constraint value is determined according to whether the constraint value meets the optimization constraint condition.
And S38, outputting an optimization variable when the convergence and the optimization constraint condition is qualified.
And when the optimization constraint condition is not qualified, the next iteration sequence quadratic programming problem is solved, when the optimization constraint condition is not qualified, S39 the motion limit is reduced to complete the motion limit updating, and the step S32 is returned to carry out the next iteration sequence quadratic programming problem solving according to the new motion limit.
Specifically, the reduced motion limit is specifically: and reducing the preset multiplying power of the optimized constraint condition of the violation constraint. Of course, the optimization constraints may also be reduced according to other rules, such as reduction by a constant or reduction by a predetermined curve, etc.
Preferably, when the optimization target and the optimization constraint condition are judged to be qualified, if the maximum curvature condition is violated, the weight of the path smoothness cost in the optimization target is increased, the curvature mutation is effectively prevented, and the optimized path is ensured to have better comfort for the vehicle.
And simultaneously in the step S36 and the step S37, judging whether the optimization variables are all at the edge of the motion interval in the preset iteration from the current iteration, and if so, enlarging the motion limit to increase. For example, whether the optimized variables in two iterations from the current iteration are all at the edge of the motion interval is judged, and specifically, the optimized variables P of the current iteration and the previous generation are judged i Whether all are in the interval
An edge. Wherein the optimized variable P is judged i Whether the interval edge is located is specifically as follows: judging an optimized variable P i Percent difference near interval edge
Or
Whether the value is less than the preset value or not, and when one of the value and the preset value is less than the preset value, the optimization variable P is judged i At the zone edge. Currently, the interval range of the optimized variable in the iterations three, four, etc. ahead from the current iteration can also be judged, and is not limited to two.
Specifically, the extended motion limits are specifically: and expanding the preset multiplying power when the optimization variable is positioned at the edge in the multi-iteration optimization. Of course, other ways of extending the motion limits may be used, such as extending with a constant or with a preset curve, etc.
Referring to fig. 5, the present invention further discloses an automatic parking path optimization apparatus 100, which includes a detection module 10, an initial path planning module 20, a problem component module 30, a problem solving module 40, and a path optimization module 50. The detection module 10 acquires obstacle information; the initial path planning module 20 generates an initial path planning track, and converts each path segment of the initial path planning track into a plurality of initial track points P i ref (x i ref ,y i ref ) I =1, 2, 3 … n; the problem component module 30 plans the coordinate P of each track point in each path of the track by the path planning module i (x i ，y i ) For optimizing variables, the maximum curvature is limited, the initial position and the position of a destination vehicle are fixed, each track point is not collided with an obstacle as an optimization constraint condition, the smoothness of a path, the path length and the distance from the initial track point are taken as optimization targets, a sequence quadratic programming problem is constructed, i =1, 2 and 3 … n are obtained, the coordinates of the initial position of the vehicle in each path are P 1 (x 1 ，y 1 ) With vehicle destination coordinates of P n (x n ，y n ),P 0 (x 0 ，y 0 ) Auxiliary points, P, added for constraining the initial course angle of the vehicle to be constant n+1 (x n+1 ，y n+1 ) And the auxiliary points are added for restraining the destination heading angle of the vehicle from being changed. Wherein the numerical value of the optimization target is the sum of the path smoothness, the path length and the weight of the distance deviating from the initial track point, and in the path smoothness, the initial coordinate P 1 (x 1 ，y 1 ) And destination coordinates P n (x n ，y n ) Are all greater than the track point coordinate P between the starting place coordinate and the destination coordinate 2 (x 2 ，y 2 ) To P n-1 (x n-1 ，y n-1 ) The weight of (c); the problem solving module 40 iteratively solves the sequential quadratic programming problem to generate track point information; and the path optimization module 50 generates an optimized planning path according to the track point information.
The invention also discloses an automatic parking path optimizing device, which comprises: one or more processors; a memory; and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by one or more processors to implement the automatic parking path optimization method as described above.
The present invention also discloses a computer-readable storage medium comprising a computer program for use in conjunction with an electronic device having a memory, the computer program being executable by a processor to implement the automatic parking path optimization method as described above.
According to the method, not only is curvature constraint taken as one of secondary planning conditions during path optimization, but also the optimized target is set as the weight summation of multiple targets such as path smoothness, and the like, the weights of different targets can be set according to actual requirements (such as different parking scenes, different vehicle types and/or different solving modes of an initial planned path trajectory), so that the optimized path is more suitable for different environments, the initial weight and the destination of the path smoothness are set to be larger, and the risk that the vehicle collides with an obstacle due to the fact that the optimized planned path is excessively smooth is effectively reduced. Furthermore, the initial path planning track is subjected to discrete processing, and discrete points of all path planning tracks are obtained and used as track points, so that the whole path planning track is subjected to all-round optimization, and the optimized whole line is ensured to accord with constraint conditions.
The above disclosure is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the scope of the present invention, therefore, the present invention is not limited by the appended claims.
Claims (15)
1. An automatic parking path optimization method is characterized in that: the method comprises the following steps:
obtaining barrier information, generating an initial path planning track, and converting each path of the initial path planning track into a plurality of discrete initial track points P i ref (x i ref ,y i ref )，i＝1、2、3…n；
Planning the coordinate P of each track point in each path of the track by the path i (x i ，y i ) For optimizing variables, constructing a sequence quadratic programming problem by taking the maximum curvature limitation, fixed initial position and destination vehicle position and non-collision of each track point with an obstacle as optimization constraint conditions, taking the smoothness of a path, the path length and the distance from the initial track point as optimization targets, and performing iterative solution on the sequence quadratic programming problem to generate track point information;
generating an optimized planning path according to the track point information;
wherein the numerical value of the optimization target is the sum of the path smoothness, the path length and the weight of the distance deviating from the initial track point, and in the path smoothness, the initial coordinate P 1 (x 1 ，y 1 ) And destination coordinates P n (x n ，y n ) Are all greater than the coordinate P of the track point between the starting location coordinate and the destination coordinate 2 (x 2 ，y 2 ) To P n-1 (x n-1 ，y n-1 ) The weight of (c).
2. The automatic parking path optimization method according to claim 1, characterized in that:
the optimization constraints that limit the maximum curvature include:
wherein the vector
C max For curvature limits, Δ l is the mean of the spacing between adjacent trace points.
3. The automatic parking path optimization method according to claim 1, characterized in that: when the iterative solution of the sequence quadratic programming problem is carried out, the optimization constraint function for limiting the maximum curvature is linearly expanded into the following steps:
and when the iterative solution of the sequence quadratic programming problem is carried out, the motion limit of the optimization variable is also limited.
4. The automatic parking path optimization method according to claim 1, characterized in that:
the constraint condition that each track point does not collide with the barrier is as follows: origin coordinate P 1 (x 1 ，y 1 ) And destination coordinates P n (x n ，y n ) Constant, locus point coordinate P 2 (x 2 ，y 2 ) To P n-1 (x n-1 ，y n-1 ) Corresponding to the initial track point P i ref (x i ref ,y i ref ) The deviation value between the X coordinate and the Y coordinate is less than or equal to
And di is the distance between the corresponding vehicle and the obstacle at each initial track point in each path of the path planning track.
5. The automatic parking path optimization method according to claim 1, characterized in that:
the course angle constraint conditions are as follows:
6. The automatic parking path optimization method according to claim 1, characterized in that:
the path smoothness cost is:
the path length penalty is:
the cost of the distance from the initial track point is:
P smooth 、P length and P ref Respectively a smoothness, a length and a penalty factor of the distance relative to the initial track point,
is a track point P i The weight of the smoothness is such that,
middle is a track point P i Weight of distance deviating from the initial track point;
the optimization target is as follows:
h is a semi-positive definite symmetric matrix, g is a gradient vector, x = { x = 0 ,y 0 ,x 1 ,y 1 ,…，x n+1 ,y n+1 } T 。
7. The automatic parking path optimization method according to claim 1, characterized in that: the iterative solution of the sequence quadratic programming problem specifically comprises the following steps: giving a motion limit of the optimized variable, performing an optimized approximate problem structure, performing sequential quadratic programming solution on the optimized approximate problem to obtain a new optimized variable, updating the optimized variable according to the new optimized variable, calculating a numerical value of an optimized target and a constraint value of an optimized constraint condition, judging whether the optimized constraint condition is qualified according to the numerical value of the optimized target, outputting the optimized variable when the optimized constraint condition is converged and qualified, performing next iterative quadratic programming problem solution when the optimized constraint condition is not converged and qualified, reducing the motion limit when the optimized constraint condition is unqualified, and performing next iterative quadratic programming problem solution.
8. The automatic parking path optimization method according to claim 7, characterized in that: the reduced exercise limit is specifically: and reducing the preset multiplying power of the optimized constraint condition of the violation constraint.
9. The automatic parking path optimization method according to claim 7, characterized in that: and when judging whether the optimization constraint condition is qualified or not according to the constraint value of the optimization constraint condition, judging whether the optimization variables are all at the edge of the motion interval of the optimization variables in multiple iterations, and if so, enlarging the motion limit to increase.
10. The automatic parking path optimization method according to claim 9, characterized in that: the extended exercise limit is specifically: and expanding the preset multiplying power when the optimization variable is positioned at the edge in the multi-iteration optimization.
11. The automatic parking path optimization method according to claim 7, characterized in that: and when judging whether the optimization target and the optimization constraint condition are qualified, if the maximum curvature condition is violated, increasing the weight of the path smoothness cost in the optimization target.
12. The automatic parking path optimization method according to claim 1, characterized in that: path planning is carried out by adopting a Hybrid A algorithm to generate the initial path planning track, and each section of path of the initial path planning track is decomposed into n +2 discrete initial track points P i ref (x i ref ,y i ref )，i＝0、1、2、3…n、n+1，P 0 ref (x 0 ref ,y 0 ref ) Auxiliary points, P, for each path segment of the path planning trajectory, which are added to constrain the initial heading angle of the vehicle to be constant n+1 ref (x n+1 ref ,y n+1 ref ) And auxiliary points which are added for restraining the constant vehicle destination course angle in each path of the path planning track.
13. An automatic parking path optimizing device characterized in that: the method comprises the following steps:
the obstacle information acquisition module acquires obstacle information;
an initial path planning module for generating an initial path planning track and converting each path of the initial path planning track into a plurality of discrete initial track points P i ref (x i ref ,y i ref )，i＝1、2、3…n；
A problem construction module for planning the coordinates P of each track point in each path of the track by the path i (x i ，y i ) For optimizing variables, the maximum curvature is limited, the initial position and the destination vehicle pose are fixed, each track point is not collided with an obstacle as an optimization constraint condition, and the smoothness, the path length and the distance from the initial track point of the path are taken as optimization targetsConstructing a sequence quadratic programming problem, i =1, 2, 3 … n, wherein the numerical value of the optimization target is the sum of the path smoothness, the path length and the weight of the distance deviating from the initial track point, and in the path smoothness, the initial coordinate P is used as the initial coordinate 1 (x 1 ，y 1 ) And destination coordinates P n (x n ，y n ) Are all greater than the track point coordinate P between the starting place coordinate and the destination coordinate 2 (x 2 ，y 2 ) To P n-1 (x n-1 ，y n-1 ) The weight of (c);
the problem solving module is used for iteratively solving the sequence quadratic programming problem to generate track point information;
and the path optimization module generates an optimized planning path according to the track point information.
14. An automatic parking path optimizing device, characterized in that: the method comprises the following steps:
one or more processors;
a memory; and
one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by one or more processors to implement the automated parking path optimization method of any of claims 1-12.
15. A computer-readable storage medium comprising a computer program for use in conjunction with an electronic device having a memory, characterized in that: the computer program is executable by a processor to implement the automated parking path optimization method according to any one of claims 1 to 12.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211190937.0A CN115456297A (en) | 2022-09-28 | 2022-09-28 | Automatic parking path optimization method and device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211190937.0A CN115456297A (en) | 2022-09-28 | 2022-09-28 | Automatic parking path optimization method and device |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115456297A true CN115456297A (en) | 2022-12-09 |
Family
ID=84306388
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211190937.0A Pending CN115456297A (en) | 2022-09-28 | 2022-09-28 | Automatic parking path optimization method and device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115456297A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117533354A (en) * | 2023-12-28 | 2024-02-09 | 安徽蔚来智驾科技有限公司 | Track generation method, driving control method, storage medium and intelligent device |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104050390A (en) * | 2014-06-30 | 2014-09-17 | 西南交通大学 | Mobile robot path planning method based on variable-dimension particle swarm membrane algorithm |
| US20190086925A1 (en) * | 2017-09-18 | 2019-03-21 | Baidu Usa Llc | Path optimization based on constrained smoothing spline for autonomous driving vehicles |
| US20200031340A1 (en) * | 2018-07-27 | 2020-01-30 | Baidu Usa Llc | Adjusting speeds along a path for autonomous driving vehicles |
| EP3685968A1 (en) * | 2019-01-22 | 2020-07-29 | Bayerische Motoren Werke Aktiengesellschaft | Robot motion planning in manufacturing |
| CN111591307A (en) * | 2020-04-15 | 2020-08-28 | 毫末智行科技有限公司 | Obstacle avoidance track planning method and system and vehicle |
| CN112000088A (en) * | 2019-05-08 | 2020-11-27 | 北京京东乾石科技有限公司 | Path planning method and device |
| CN112277932A (en) * | 2020-10-21 | 2021-01-29 | 深圳市德航智能技术有限公司 | Agricultural machinery automatic driving system key technology research and development based on Beidou positioning |
| CN112862204A (en) * | 2021-02-23 | 2021-05-28 | 国汽(北京)智能网联汽车研究院有限公司 | Path planning method, system, computer equipment and readable storage medium |
| CN113050652A (en) * | 2021-03-25 | 2021-06-29 | 上海海事大学 | Trajectory planning method for automatic berthing of intelligent ship |
| CN113306549A (en) * | 2021-06-17 | 2021-08-27 | 英博超算(南京)科技有限公司 | Automatic parking trajectory planning algorithm |
| CN114987492A (en) * | 2021-03-01 | 2022-09-02 | 武汉智行者科技有限公司 | Curvature constraint-considered automatic driving vehicle track smooth optimization method |
-
2022
- 2022-09-28 CN CN202211190937.0A patent/CN115456297A/en active Pending
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104050390A (en) * | 2014-06-30 | 2014-09-17 | 西南交通大学 | Mobile robot path planning method based on variable-dimension particle swarm membrane algorithm |
| US20190086925A1 (en) * | 2017-09-18 | 2019-03-21 | Baidu Usa Llc | Path optimization based on constrained smoothing spline for autonomous driving vehicles |
| US20200031340A1 (en) * | 2018-07-27 | 2020-01-30 | Baidu Usa Llc | Adjusting speeds along a path for autonomous driving vehicles |
| EP3685968A1 (en) * | 2019-01-22 | 2020-07-29 | Bayerische Motoren Werke Aktiengesellschaft | Robot motion planning in manufacturing |
| CN112000088A (en) * | 2019-05-08 | 2020-11-27 | 北京京东乾石科技有限公司 | Path planning method and device |
| CN111591307A (en) * | 2020-04-15 | 2020-08-28 | 毫末智行科技有限公司 | Obstacle avoidance track planning method and system and vehicle |
| CN112277932A (en) * | 2020-10-21 | 2021-01-29 | 深圳市德航智能技术有限公司 | Agricultural machinery automatic driving system key technology research and development based on Beidou positioning |
| CN112862204A (en) * | 2021-02-23 | 2021-05-28 | 国汽(北京)智能网联汽车研究院有限公司 | Path planning method, system, computer equipment and readable storage medium |
| CN114987492A (en) * | 2021-03-01 | 2022-09-02 | 武汉智行者科技有限公司 | Curvature constraint-considered automatic driving vehicle track smooth optimization method |
| CN113050652A (en) * | 2021-03-25 | 2021-06-29 | 上海海事大学 | Trajectory planning method for automatic berthing of intelligent ship |
| CN113306549A (en) * | 2021-06-17 | 2021-08-27 | 英博超算(南京)科技有限公司 | Automatic parking trajectory planning algorithm |
Non-Patent Citations (4)
| Title |
|---|
| 薄荷: "apollo 参考线平滑算法解析", pages 1 - 16, Retrieved from the Internet <URL:https://zhuanlan.zhihu.com/p/565854845> * |
| 赵海兰;高松;孙宾宾;武哲;唐鹏;: "全自动平行泊车路径规划方法研究", 科学技术与工程, no. 07, 8 March 2017 (2017-03-08) * |
| 顾元宪, 马红艳, 姜成, 亢战, 张洪武: "海洋平台结构动力响应优化设计与灵敏度分析", 海洋工程, no. 01, 30 April 2001 (2001-04-30) * |
| 顾元宪等: "海洋平台结构动力响应优化设计与灵敏度分析", 《海洋工程》, vol. 19, no. 1, pages 7 - 9 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117533354A (en) * | 2023-12-28 | 2024-02-09 | 安徽蔚来智驾科技有限公司 | Track generation method, driving control method, storage medium and intelligent device |
| CN117533354B (en) * | 2023-12-28 | 2024-04-02 | 安徽蔚来智驾科技有限公司 | Track generation method, driving control method, storage medium and intelligent device |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110316193B (en) | Preview distance setting method, device, equipment and computer readable storage medium | |
| CN107063280B (en) | Intelligent vehicle path planning system and method based on control sampling | |
| CN112622932B (en) | Automatic driving track-changing planning algorithm based on heuristic search of potential energy field | |
| CN107766405B (en) | Automatic vehicle road model definition system | |
| Muller et al. | Continuous curvature trajectory design and feedforward control for parking a car | |
| CN111290385A (en) | Robot path planning method, robot, electronic device and storage medium | |
| EP3948440A1 (en) | Nonlinear optimization method for stochastic predictive control | |
| US20160090087A1 (en) | Cruise control system and method | |
| Xiong et al. | Application improvement of A* algorithm in intelligent vehicle trajectory planning | |
| CN115456297A (en) | Automatic parking path optimization method and device | |
| CN111121777A (en) | Unmanned equipment trajectory planning method and device, electronic equipment and storage medium | |
| CN111746525B (en) | Parking path planning method, device, equipment and storage medium | |
| US20230022510A1 (en) | Stochastic Nonlinear Predictive Controller and Method based on Uncertainty Propagation by Gaussian-assumed Density Filters | |
| CN110750850A (en) | Three-dimensional profile optimization design method, system and medium under strong constraint complex task condition | |
| Chen et al. | Path planning for autonomous vehicle based on a two-layered planning model in complex environment | |
| Zhou et al. | A novel reentry trajectory generation method using improved particle swarm optimization | |
| CN111746523A (en) | Vehicle parking path planning method and device, vehicle and storage medium | |
| Qie et al. | An improved model predictive control-based trajectory planning method for automated driving vehicles under uncertainty environments | |
| CN117141520B (en) | Real-time track planning method, device and equipment | |
| CN116136417B (en) | Unmanned vehicle local path planning method facing off-road environment | |
| Kim et al. | Dynamic bipedal walking using real-time optimization of center of mass motion and capture point-based stability controller | |
| CN115534998A (en) | Automatic driving integrated decision-making method and device, vehicle and storage medium | |
| JP2021008224A (en) | Vehicle control apparatus, parking assistance apparatus, vehicle control method, and parking assistance method | |
| CN114228748A (en) | Human-like automatic driving track planning method based on geometric path generation | |
| Jiang et al. | Multi-target tracking algorithm of vehicle-mounted millimeter-wave radar based on IMM-JPDA-STUKF |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |