CN115456297B - Automatic parking path optimization method and device - Google Patents
Automatic parking path optimization method and device Download PDFInfo
- Publication number
- CN115456297B CN115456297B CN202211190937.0A CN202211190937A CN115456297B CN 115456297 B CN115456297 B CN 115456297B CN 202211190937 A CN202211190937 A CN 202211190937A CN 115456297 B CN115456297 B CN 115456297B
- Authority
- CN
- China
- Prior art keywords
- path
- optimization
- track
- ref
- initial
- 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.)
- Active
Links
- 238000005457 optimization Methods 0.000 title claims abstract description 174
- 238000000034 method Methods 0.000 title claims abstract description 45
- 230000004888 barrier function Effects 0.000 claims abstract description 4
- 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
- 238000009499 grossing Methods 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 230000009467 reduction Effects 0.000 description 2
- 230000007704 transition Effects 0.000 description 2
- 230000003247 decreasing effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000035772 mutation Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
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
Landscapes
- Business, Economics & Management (AREA)
- Human Resources & Organizations (AREA)
- Engineering & Computer Science (AREA)
- Strategic Management (AREA)
- Economics (AREA)
- Marketing (AREA)
- Game Theory and Decision Science (AREA)
- Entrepreneurship & Innovation (AREA)
- Development Economics (AREA)
- Operations Research (AREA)
- Quality & Reliability (AREA)
- Tourism & Hospitality (AREA)
- Physics & Mathematics (AREA)
- General Business, Economics & Management (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a method and a device for optimizing an automatic parking path, 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(xi ref,yi ref); taking each track point coordinate P i(xi,yi) of each path in the path planning track as an optimization variable, limiting the maximum curvature, fixing the pose of the origin and the destination vehicle, taking each track point not collided with an obstacle as an optimization constraint condition, taking the smoothness of the path, the length of the path and the distance away from the initial track point as an optimization target, constructing a sequence quadratic programming problem, and carrying out iterative solution to generate track point information; generating an optimized planning path according to the track point information; the values of the optimization targets are summed up for weights of the optimization targets, and in the path smoothness, the weights of the start position coordinates and the destination coordinates are larger than the weights of other track point coordinates between the start position coordinates and the destination 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 is free from abrupt changes. At present, a curve transition method and an optimization method mainly exist, the curve transition is generally carried out on a curvature convex change part only, the gradient-free optimization algorithm cannot guarantee the solving efficiency, and most of the current smoothing algorithms possibly have larger deviation between a local track and a target point and excessively smooth, so that the risk of collision between a vehicle and an obstacle exists. Because the curvature constraint of the path is nonlinear, the curvature constraint cannot be added in the quadratic programming, and the existing optimized planning path often has the problems of abrupt curvature change and excessive smoothness of the vehicle, so that the vehicle collides with an obstacle.
For this reason, in patent publication CN112277932a, an automatic driving parking path planning method is disclosed, in which a path curvature constraint is introduced into a constraint condition and an optimization target of an optimization function, so that the optimized parking trajectory and curvature are continuous, and the steering wheel changes continuously when the vehicle runs along the trajectory, without stopping. However, the optimization method only optimizes a small number of points of the initial path, cannot ensure that the whole path meets constraint conditions and is limited to a certain range of parallel parking planning only by the initial position of the vehicle, and meanwhile, the optimization method (geometric method) in the patent can only be optimized, has no universality, is difficult to use in actual parking path planning, and cannot solve the problem of steering wheel steering greatly when the vehicle is stationary during optimization.
Therefore, there is an urgent need for an automatic parking path planning method and apparatus that can solve the above-mentioned 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 excessive smoothness of an optimized planned path.
In order to achieve the above object, the present invention discloses an automatic parking path optimizing method, comprising: 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(xi ref, yi ref, =1, 2 and 3 … n; taking each track point coordinate P i（xi,yi) in each section of the path planning track as an optimization variable, limiting the maximum curvature, fixing the pose of the origin and the destination vehicle, taking each track point not collided with an obstacle as an optimization constraint condition, taking the smoothness of the path, the length of the path and the distance away from the initial track point as an optimization target, constructing a sequence quadratic programming problem, and carrying out 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 value of the optimization target is a sum of weights of path smoothness, path length and distance from the initial track point, and the weights of the start point coordinate P 1（x1,y1) and the destination coordinate P n（xn,yn) are larger than the weights of the track point coordinates P 2（x2,y2) to P n-1（xn-1,yn-1) between the start point coordinate and the destination coordinate in the path smoothness.
Preferably, the optimization constraint that limits the maximum curvature includes:
；
Wherein the vector is C max is a curvature limit value,/>Is the average value of the spacing between adjacent track points.
Preferably, when the iterative solution of the sequence quadratic programming problem is performed, the optimization constraint function limiting the maximum curvature is linearly expanded into:
，/>，/> Optimizing the solution obtained by iteration in the last step for sequence quadratic programming; and when the iterative solution of the sequence quadratic programming problem is carried out, the motion limit of the optimized variable is also limited.
Preferably, the constraint that each track point does not collide with an obstacle is: origin coordinates P 1（x1,y1) and destination coordinates P n（xn,yn) are unchanged, and the offset values of the X-coordinate and the Y-coordinate between the trajectory point coordinates P 2（x2,y2) to P n-1（xn-1,yn-1) and the initial trajectory point P i ref(xi ref, yi ref) are equal to or lessDi is the nearest 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:
，/> And/> Set heading angle set at the origin and destination according to the origin and destination vehicle pose, respectively, P 0（x0,y0) is an auxiliary point for constraining the constant addition of the origin heading angle of the vehicle, P n+1（xn+1,yn+1) is an auxiliary point for constraining the constant addition of the destination heading angle of the vehicle.
Preferably, the path smoothness cost is:
；
The path length cost is:
；
The distance cost of deviating from the initial track point is:
；
p smooth、Plength and P ref are penalty factors for smoothness, length and distance from the initial trajectory point, Is the weight of the smoothness of the track point P i,/>The middle is the weight of the track point P i deviating from the initial track point distance;
The optimization targets are as follows:
；
H is a semi-positive definite symmetric matrix, g is a gradient vector, 。
Preferably, the iterative solution to the sequence quadratic programming problem specifically includes: giving a motion limit of the optimization variable and constructing an optimization approximation problem, carrying out sequential quadratic programming solving on the optimization approximation problem to obtain a new optimization variable, updating the optimization variable according to the new optimization variable, calculating the numerical value of an optimization target and the constraint value of an optimization constraint condition, judging whether the optimization constraint condition is qualified according to the numerical value of the optimization target, outputting the optimization variable when the optimization constraint condition is qualified and converged, carrying out next iterative sequential quadratic programming problem solving when the optimization constraint condition is not converged and qualified, and reducing the motion limit and carrying out next iterative sequential quadratic programming problem solving when the optimization constraint condition is not qualified.
Specifically, the curtailed motion limit is specifically: and reducing the preset multiplying power of the optimization constraint condition which violates the constraint.
More preferably, when judging that the optimization constraint condition is qualified 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, if so, enlarging the motion limit and increasing so as to improve the solving efficiency.
Specifically, the expansion motion limit is specifically: and expanding the preset multiplying power when the optimization variable is at the edge in the multiple iterative optimization.
Specifically, when judging whether the optimization target and the optimization constraint condition are qualified or not, if the maximum curvature condition is violated, adding the weight of the path smoothness cost to the optimization target.
Specifically, a Hybrid a algorithm is adopted to perform path planning to generate the initial path planning track, each path of the initial path planning track is decomposed into n+2 discrete initial track points Pi ref(xi ref, yi ref),i=0、1、2、3…n、n+1,P0 ref(x0 ref, y0 ref) which are auxiliary points added with unchanged heading angle of the constrained vehicle in each path of the path planning track, and P n+1 ref(xn+1 ref, yn+1 ref) which are auxiliary points added with unchanged heading angle of the constrained vehicle destination in each path of the path planning track. The Hybrid A algorithm is used for planning the initial path, so that 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.
The invention also discloses an automatic parking path optimizing device, which comprises: the detection module is used for acquiring barrier information; the initial path planning module generates an initial path planning track and converts each path of the initial path planning track into a plurality of initial track points P i ref(xi ref, yi ref); the problem component module takes the coordinates P i（xi,yi of each track point in each section of path of the path planning track as an optimization variable, limits the maximum curvature, fixes the pose of the starting place and the pose of the destination vehicle, takes each track point not to collide with an obstacle as an optimization constraint condition, takes the smoothness of the path, the length of the path and the distance away from the initial track point as an optimization target, and constructs a sequence quadratic programming problem, wherein i=1, 2 and 3 … n; the problem solving module is used for carrying out iterative solving on the sequence quadratic programming problem to generate track point information; the path optimization module generates an optimized planning path according to the track point information; wherein the value of the optimization target is a sum of weights of path smoothness, path length and distance from the initial track point, and the weights of the start point coordinate P 1（x1,y1) and the destination coordinate P n（xn,yn) are larger than the weights of the track point coordinates P 2（x2,y2) to P n-1（xn-1,yn-1) between the start point coordinate and the destination coordinate in the path smoothness.
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 the one or more processors to implement the automated parking path optimization method as described above.
The invention also discloses a computer readable storage medium comprising a computer program for use in connection with an electronic device having a memory, the computer program being executable by a processor to implement the automated parking path optimization method as described above.
Compared with the prior art, the method has the advantages that curvature constraint is used as one of the sequence quadratic programming conditions in path optimization, the optimization targets are set to be the sum of weights of a plurality of targets such as path smoothness, so that the optimized path is more suitable for different environments, the initial place and the destination of the weights of the path smoothness are set larger, and the risk of collision between a vehicle and an obstacle caused by excessive smoothness of the optimized planned path is effectively reduced. Furthermore, the invention carries out discrete processing on each section of path of the initial path planning track to obtain the discrete points of all path planning tracks as track points so as to carry out all-round optimization on the whole path planning track, ensure that the optimized whole line meets constraint conditions, and simultaneously, the invention is suitable for the path smoothing processing of the initial path planning track obtained by a plurality of path planning methods and has strong applicability.
Drawings
Fig. 1 is a flowchart of an automatic parking path optimizing method of the present invention.
Fig. 2 is a graph of a path of a segment of an initial path planning trajectory of the present invention.
FIG. 3 is a graph of a path segment of a path planning trajectory obtained by a sequential quadratic programming solution of the present invention.
FIG. 4 is a flow chart of the sequential quadratic programming solution of the present invention.
Fig. 5 is a structural view of the automatic parking path optimizing apparatus of the present invention.
Detailed Description
In order to describe the technical content, the constructional features, the achieved objects and effects of the present invention in detail, the following description is made in connection with the embodiments and the accompanying drawings.
Referring to fig. 1, the invention discloses an automatic parking path optimization method, which comprises steps S11-S14.
S11, obstacle information is acquired, an initial path planning track is generated, and each path of the initial path planning track is converted into a plurality of discrete initial track points P i ref(xi ref, yi ref. i=1, 2,3 … n. The specific method for acquiring the obstacle information can be obtained through detection of a related sensor, can be input through external equipment, and can also be directly acquired, for example, can be acquired according to map information.
The generated initial path planning track can have one section or can be divided into multiple sections, and the whole initial path planning track is particularly divided into corresponding sections of paths according to the driving direction.
Wherein, the invention converts each path of the initial path planning track into a plurality of discrete initial track points P i ref(xi ref, yi ref). The initial track points are a plurality of discrete points on the whole path of the track for path planning. The number of the track points in each section of the path can be equal or unequal, and the specific number of the track points is determined by the set distance range between the adjacent track points or according to the actual complex condition of the path.
Referring to fig. 2, the present invention performs path planning by using Hybrid a algorithm to generate the initial path planning track, and decomposes each path of the initial path planning track into n+2 discrete initial track points P i ref(xi ref, yi ref), i=0, 1,2, 3 … n, n+1.
The method combines the Hybrid A algorithm with the sequence quadratic programming algorithm, optimizes the path programming track acquired by the Hybrid A algorithm in all directions, can ensure that the optimized whole line meets constraint conditions, is suitable for parking in any parking space, and has wide applicability. Wherein, the initial track point P 0 ref(x0 ref, y0 ref) is an auxiliary point added to the initial path planning track, which restricts the initial heading angle of the vehicle, the slope of the connecting line between P 0 ref(x0 ref, y0 ref) and P 2 ref(x2 ref, y2 ref) is equal to the slope corresponding to the initial heading angle of the vehicle, and the distance between P 0 ref(x0 ref, y0 ref) and P 1 ref(x1 ref, y1 ref) is equal to the distance between P 1 ref(x1 ref, y1 ref) and P 2 ref(x2 ref, y2 ref).
Wherein, the initial track point P n+1 ref(xn+1 ref, yn+1 ref) is an auxiliary point added to the initial path planning track, which constrains the heading angle of the destination of the vehicle 200 to be unchanged, the slope of the connecting line between P n-1 ref(xn-1 ref, yn-1 ref) and P n+1 ref(xn+1 ref, yn+1 ref) is equal to the corresponding slope of the heading angle of the destination of the vehicle, and the distance between P n+1 ref(xn+1 ref, yn+1 ref) and P n ref(xn ref, yn ref) is equal to the distance between P 1 ref(x1 ref, y1 ref) and P 2 ref(x2 ref, y2 ref).
S12, taking each track point coordinate P i（xi,yi in each section of the path planning track as an optimization variable, limiting the maximum curvature, fixing the pose of the starting place and the destination vehicle (fixing the coordinate points of the starting place and the destination and fixing the course angle), taking each track point not colliding with an obstacle as an optimization constraint condition, taking the smoothness of the path, the length of the path and the distance away from the initial track point (the deviation value of the track point coordinate in each section of the path planning track and the initial track point coordinate in the corresponding section of the path of the initial path planning track) as an optimization target, and constructing a sequence secondary planning problem. Wherein i=0, 1, 2, 3 … n, n+1.
Referring to fig. 3, each section of path of the initial path planning track is optimized through a sequence quadratic programming algorithm, so that path track points of a section corresponding to the optimized path planning track are obtained. In each path of the path planning track, the vehicle starting point coordinate is P 1（x1,y1), the vehicle destination coordinate is P n（xn,yn), the track point P 0（x0,y0) is an auxiliary point which is added by constraining the vehicle starting point course angle unchanged, and the track point P n+1（xn+1,yn+1) is an auxiliary point which is added by constraining the vehicle destination course angle unchanged.
The numerical value of the optimization target is the sum of weights of the path smoothness, the path length and the distance away from the initial track point. The weights of different optimization targets (including the path smoothness, the path length, and the distance from the initial track point) may be adjusted according to the actual parking environment, and are not fixed, for example, when the values of the distances d i between the obstacle and the corresponding vehicles 200 at the respective initial track points are smaller than a preset value, the weights of the distances from the initial track points are increased and/or the weights of the path lengths are decreased on the basis of the initial weights.
Step S12 further includes calculating a distance d i according to the distance between the initial trajectory point and the obstacle. The distances between the four edge line segments of the vehicle 200 and the obstacle line segments are solved, and the minimum value is taken as the shortest distance d i between the vehicle 200 and the obstacle.
In the same optimization target, the weights of different track points can be the same or different, and the weights can be set according to actual needs. For example, in the present embodiment, in the path smoothness, the weights of the origin coordinates P 1（x1,y1) and the destination coordinates P n（xn,yn) are each larger than the weights of the trajectory point coordinates P 2（x2,y2) to P n-1（xn-1,yn-1) between the origin coordinates and the destination coordinates.
In the path smoothness of the present embodiment, the weights between the track point coordinates P 2（x2,y2) to P n-1（xn-1,yn-1) may be the same, or in the path smoothness, the weights of the first m track points may be gradually reduced, the weights of the last m track points may be gradually increased, for example, the weights of the track points P 1（x1,y1) to the track points P 3（x3,y3) may be gradually reduced, the weights of the track points P n-2（xn-2,yn-2) to the track points P n（xn,yn) may be gradually increased, m is an integer greater than or equal to 1, and may be 1,2, 3, etc., and the value thereof is less than or equal to n/2.
S13, carrying out iterative solution on the sequence quadratic programming problem to generate track point information.
S14, generating an optimized planning path according to the track points P 1（x1,y1) to P n（xn,yn) in the track point information in all the segment paths.
When the initial path planning track has multiple paths, in step S14, a final optimized total planning path is generated according to the track points P 1（x1,y1) to P n（xn,yn) in the paths of all the segments, wherein the final optimized planning path can be directly generated according to the track points P 1（x1,y1) to P n（xn,yn) in the initial path planning track of all the segments, or each segment of optimized planning path can be generated according to the track points P 1（x1,y1) to P n（xn,yn) in the initial path planning track of each segment, and then all the segments of optimized planning paths are combined into the total planning path.
In step S12, the specific contents and formulas of the optimization constraint condition are as follows:
the optimization constraints limiting the maximum curvature are:
；
Wherein the vector is C max is a curvature limit value,/>Is the average value of the spacing between adjacent track points. /(I)In particular to/>, which is obtained by solving the previous step of optimization iterationIn the track points P 1(x1, y1) to P n(xn, yn). When the current time is the first generation optimization iteration, the method comprises the following steps ofIn particular, the average of the distances between adjacent track points between the initial track points P 1 ref(x1 ref, y1 ref) to P n ref(xn ref, yn ref).
The constraint condition that each track point does not collide with an obstacle is as follows: origin coordinates P 1（x1,y1) and destination coordinates P n（xn,yn) are unchanged, and the offset values of the X-coordinate and the Y-coordinate between the trajectory point coordinates P 2（x2,y2) to P n-1（xn-1,yn-1) and the initial trajectory point P i ref(xi ref, yi ref) are equal to or lessD i is the nearest distance between the corresponding vehicle and the obstacle at each track point in each path of the path planning track. The deviation value of the X coordinate and the Y coordinate between the track point coordinate P 0（x0,y0) and the initial track point coordinate P 0 ref(x0 ref, y0 ref) is less than or equal to/>，/>Setting an upper limit value for a preset coordinate position offset value,/>Greater than or equal to/>。
Specifically, the constraint condition formula is:
In the method, in the process of the invention, Wherein:
。
The heading angle constraint conditions are as follows:
，/> And/> The heading angle set values at the start and destination according to the pose of the start and destination vehicles, respectively.
In step S12, the specific content and formula of the optimization objective are as follows:
The path smoothness is: 。
The path smoothness cost is:
。
The path length is: 。
The path length cost is:
。
the distance from the initial track point is as follows: 。
The distance cost of deviating from the initial track point is:
。
P smooth、Plength and P ref are penalty factors for smoothness, length, and distance from the initial trajectory point, respectively, and are preset constants. Is the weight of the smoothness of the track point P i,/>The middle is the weight of the track point P i deviating from the distance of the initial track point, is a numerical value set during sequence quadratic programming, and the initial value is a preset value. In this embodiment, the weight ratio of the smoothness, the path length and the distance from the initial track point of each optimization target path is: /(I)：1：/>. Of course, the weights of the respective optimization targets are limited in the above formula, and the weights of the path lengths may be a set value instead of being fixed on the number 1.
The optimization targets are as follows:
；
H is The semi-positive symmetry matrix, g is 2n+4-dimensional gradient vector, is a constant calculated by the above formula. /(I)The method is inversion of a matrix of track point coordinates P 0- Pn+1, and is a variable of sequence quadratic programming.
In step S12, according to the constraint conditions and the optimization objective, an optimization problem may be established:
In step S13, when performing an iterative solution of the sequential quadratic programming problem,
Linearly expanding an optimization constraint function limiting the maximum curvature into:
，/>。
planning a solution obtained by the last optimization iteration for the sequence twice; and when the iterative solution of the sequence quadratic programming problem is carried out, the motion limit of the optimized variable is also limited.
In step S13, the iterative solution to the sequence quadratic programming problem specifically includes step S31-step S39:
S31 given an optimization variable P i（xi,yi) initial motion limits . In this step, other individual parameters (including/>、/>Parameters such as Cmax, etc.).
S32 performs optimization approximation problem construction (QP). The method comprises the following steps:
and planning the solution obtained by the last optimization iteration for the sequence twice.
S33, carrying out sequential quadratic programming solving on the optimization approximation problem to obtain a new optimization variable.
S34, updating the optimization variables according to the new optimization variables.
S35, calculating the numerical value of the optimization target and the constraint value of the optimization constraint condition.
S36, judging whether convergence is achieved according to the value of the optimization target. And judging convergence when the value of the optimization target is smaller than the value of the optimization target of the previous iteration and the difference value is smaller than a preset value, and judging non-convergence when the difference value of the optimization target value and the value of the optimization target of the previous iteration is larger than the preset value or the value of the optimization target of the previous iteration is larger than the value of the optimization target of the previous iteration.
S37, judging whether the optimization constraint condition is qualified or not according to the constraint value of the optimization constraint condition. Specifically, whether the constraint value accords with the optimization constraint condition is determined.
And S38, outputting an optimization variable when the convergence and optimization constraint conditions are qualified.
And (3) carrying out the next iteration sequence quadratic programming problem solving when the optimization constraint condition is not converged and qualified, reducing the motion limit to finish the motion limit updating by S39 when the optimization constraint condition is not qualified, and returning to the step (S32) according to the new motion limit to carry out the next iteration sequence quadratic programming problem solving.
Specifically, the curtailed motion limit is specifically: and reducing the preset multiplying power of the optimization constraint condition which violates the constraint. Of course, the optimization constraints may also be reduced according to other rules, such as a constant reduction or a preset curve reduction, etc.
Preferably, 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 increased in the optimization target, curvature mutation is effectively prevented, and the comfort of the optimized path to the vehicle is ensured to be better.
And (3) in the step S36 and the step S37, judging whether the optimization variables in the preset iterations from the current iteration to the preset iterations are all at the edge of the motion interval, and if so, expanding the motion limit and increasing. For example, it is determined whether the optimization variables are both at the edge of the motion interval in two iterations from the current iteration, specifically, whether the optimization variables P i of the current and previous iterations are both at the intervalEdges. The determining whether the optimization variable P i is at the interval edge specifically includes: judging the optimized variable/>Percent phase difference near interval edge/>Or/>Judging whether the optimization variable is smaller than a preset value or not, and judging the optimization variable/>, when one of the optimization variable and the preset value is smaller than the preset valueAt the interval edge. Currently, the interval range of the optimization variable in the equal number of iterations, three and four times, from the current iteration can also be judged, and is not limited to two times.
Specifically, the expansion motion limit is specifically: and expanding the preset multiplying power when the optimization variable is at the edge in the multiple iterative optimization. Of course, other ways of expanding the motion limits may be used, such as expanding with a constant or expanding with a preset curve, etc.
Referring to fig. 5, the invention also discloses an automatic parking path optimizing device 100, which comprises a detection module 10, an initial path planning module 20, a problem component module 30, a problem solving module 40 and a path optimizing 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 of the initial path planning track into a plurality of initial track points P i ref(xi ref, yi ref), i=1, 2,3 … n; the problem component module 30 uses the coordinates P i（xi,yi of each track point in each path of the path planning track as an optimization variable, uses the maximum curvature limitation, the fixed position of the origin and the destination vehicle, the non-collision of each track point and the obstacle as an optimization constraint condition, uses the smoothness of the path, the path length and the distance away from the initial track point as an optimization target, constructs a sequence quadratic programming problem, i=1, 2,3 … n, each path has the coordinates P 1（x1,y1 of the origin of the vehicle), has the coordinates P n（xn,yn）,P0（x0,y0 of the destination of the vehicle as an auxiliary point for the constant addition of the heading angle of the origin of the constraint vehicle, and has the coordinates P n+1（xn+1,yn+1) as an auxiliary point for the constant addition of the heading angle of the destination of the constraint vehicle. Wherein the value of the optimization target is the sum of the weights of the path smoothness, the path length and the distance from the initial track point, and the weights of the start point coordinate P 1（x1,y1) and the destination coordinate P n（xn,yn) in the path smoothness are larger than the weights of the track point coordinates P 2（x2,y2) to P n-1（xn-1,yn-1) between the start point coordinate and the destination coordinate; the problem solving module 40 carries out iterative solving on the sequence quadratic programming problem to generate track point information; the path optimization module 50 generates an optimized planned 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 the one or more processors to implement the automated parking path optimization method as described above.
The invention also discloses a computer readable storage medium comprising a computer program for use in connection with an electronic device having a memory, the computer program being executable by a processor to implement the automated parking path optimization method as described above.
The method and the system not only take curvature constraint as one of quadratic programming conditions in path optimization, but also set the optimization target as the weight summation of a plurality of targets such as path smoothness, and 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 initial planning path trajectories), so that the optimized path is more suitable for different environments, the setting of the starting place and the destination of the weight of the path smoothness is larger, and the risk of collision between the vehicle and the obstacle caused by excessive smoothness of the optimized planning path is effectively reduced. Furthermore, the invention carries out discrete processing on the initial path planning track to obtain the discrete points of all path planning tracks as track points so as to carry out omnibearing optimization on the whole path planning track and ensure that the optimized whole line meets constraint conditions.
The foregoing description of the preferred embodiments of the present invention is not intended to limit the scope of the claims, which follow, as defined in the claims.
Claims (15)
1. An automatic parking path optimization method is characterized in that: comprising 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(xi ref, yi ref, =1, 2 and 3 … n;
Taking each track point coordinate P i（xi,yi) in each section of the path planning track as an optimization variable, limiting the maximum curvature, fixing the pose of the origin and the destination vehicle, taking each track point not collided with an obstacle as an optimization constraint condition, taking the smoothness of the path, the length of the path and the distance away from the initial track point as an optimization target, constructing a sequence quadratic programming problem, and carrying out iterative solution on the sequence quadratic programming problem to generate track point information; the path smoothness cost is: P smooth is a penalty factor for smoothness,/> The smoothness of the track point P i is weighted;
generating an optimized planning path according to the track point information;
The values of the optimization targets are the sum of weights of the path smoothness, the path length and the distance away from the initial track points, the weights of the start point coordinate P 1（x1,y1) and the destination point coordinate P n（xn,yn) in the path smoothness are larger than the weights of track point coordinates P 2（x2,y2) to P n-1（x n-1,y n-1) between the start point coordinate and the destination point coordinate, the weights of the first m track points in the track point coordinates P2 (x 2, y 2) to Pn-1 (xn-1, yn-1) are gradually reduced, the weights of the last m track points are gradually increased, and m is an integer which is larger than or equal to 1 and smaller than or equal to n/2.
2. The automated parking path optimization method of claim 1, wherein:
Optimization constraints that limit the maximum curvature include:
；
Wherein the vector is C max is a curvature limit value,/>Is the average value of the spacing between adjacent track points.
3. The automated parking path optimization method of claim 1, wherein: when the iterative solution of the sequence quadratic programming problem is carried out, the optimization constraint function limiting the maximum curvature is linearly unfolded as follows:
，/>， optimizing the solution obtained by iteration in the last step for sequence quadratic programming;
And when the iterative solution of the sequence quadratic programming problem is carried out, the motion limit of the optimized variable is also limited.
4. The automated parking path optimization method of claim 1, wherein:
The constraint condition that each track point does not collide with an obstacle is as follows: the start point coordinate P 1（x1,y1) and the destination coordinate P n（xn,yn) are unchanged, and the deviation values of the X coordinate and the Y coordinate between the track point coordinates P 2（x2,y2) to P n-1（x n-1,y n-1) and the corresponding initial track point P i ref(xi ref, yi ref) are less than or equal to Di is the nearest distance between the corresponding vehicle and the obstacle at each initial track point in each path of the path planning track.
5. The automated parking path optimization method of claim 1, wherein:
The heading angle constraint conditions are as follows:
，/> And/> The set heading angle set values of the starting place and the destination place are set according to the pose of the starting place and the pose of the destination vehicle respectively; p 0（x0,y0) auxiliary points added for constraining the constant heading angle of the vehicle in each section of the path planning track, and P n+1（x n+1,y n+1) auxiliary points added for constraining the constant heading angle of the vehicle in each section of the path planning track.
6. The automated parking path optimization method of claim 1, wherein: the path length cost is:
；
The distance cost of deviating from the initial track point is:
；
P length and P ref are penalty factors for length and distance from the initial trajectory point, respectively, as The middle is the weight of the track point P i deviating from the initial track point distance;
The optimization targets are as follows:
；
H is a semi-positive definite symmetric matrix, g is a gradient vector, 。
7. The automated parking path optimization method of claim 1, wherein: the iterative solution to the sequence quadratic programming problem specifically comprises: giving a motion limit of the optimization variable and constructing an optimization approximation problem, carrying out sequential quadratic programming solving on the optimization approximation problem to obtain a new optimization variable, updating the optimization variable according to the new optimization variable, calculating the numerical value of an optimization target and the constraint value of an optimization constraint condition, judging whether the optimization constraint condition is qualified according to the numerical value of the optimization target, outputting the optimization variable when the optimization constraint condition is qualified and converged, carrying out next iteration quadratic programming problem solving when the optimization constraint condition is unqualified and reducing the motion limit when the optimization constraint condition is unqualified, and carrying out next iteration quadratic programming problem solving.
8. The automated parking path optimization method of claim 7, wherein: the reduced motion limit is specifically: and reducing the preset multiplying power of the optimization constraint condition which violates the constraint.
9. The automated parking path optimization method of claim 7, wherein: when judging that the optimization constraint condition is qualified 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 and increasing.
10. The automated parking path optimization method of claim 9, wherein: the expansion movement limit is specifically: and expanding the preset multiplying power when the optimization variable is at the edge in the multiple iterative optimization.
11. The automated parking path optimization method of claim 7, wherein: and when judging whether the optimization target and the optimization constraint condition are qualified or not, if the maximum curvature condition is violated, adding the weight of the path smoothness cost to the optimization target.
12. The automated parking path optimization method of claim 1, wherein: and carrying out path planning by adopting a Hybrid A algorithm to generate the initial path planning track, decomposing each path of the initial path planning track into n+2 discrete initial track points Pi ref(xi ref, yi ref),i=0、1、2、3…n、n+1,P0 ref(x0 ref, y0 ref) which are auxiliary points added with unchanged heading angles of the restraint vehicles in each path of the path planning track, and P n+1 ref(xn+1 ref, yn+1 ref) which are auxiliary points added with unchanged heading angles of the restraint vehicles in each path of the path planning track.
13. An automatic parking path optimizing device, which is characterized in that: comprising the following steps:
the obstacle information acquisition module acquires obstacle information;
The initial path planning module generates an initial path planning track, and converts each path of the initial path planning track into a plurality of discrete initial track points P i ref(xi ref, yi ref), i=1, 2 and 3 … n;
The problem construction module takes each track point coordinate P i（xi,yi in each section of the path planning track as an optimization variable, limits the maximum curvature, fixes the position of the origin and the destination vehicle, takes each track point not to collide with an obstacle as an optimization constraint condition, takes the smoothness of the path, the path length and the distance away from the initial track point as an optimization target, constructs a sequence quadratic programming problem, i=1, 2 and 3 … n, wherein the numerical value of the optimization target is the sum of the weights of the path smoothness, the path length and the distance away from the initial track point, and the path smoothness cost is: P smooth is a penalty factor for smoothness,/> The smoothness of the track point P i is weighted; and in the path smoothness, the weights of the origin coordinates P 1（x1,y1) and the destination coordinates P n（xn,yn) are each larger than the weights of the trajectory point coordinates P 2（x2,y2) to P n-1（x n-1,y n-1) between the origin coordinates and the destination coordinates; in the track point coordinates P2 (x 2, y 2) to Pn-1 (xn-1, yn-1), the weights of the first m track points gradually decrease, and the weights of the last m track points gradually increase, wherein m is an integer greater than or equal to 1 and less than or equal to n/2;
The problem solving module is used for carrying out iterative solving on 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, which is characterized in that: comprising 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 are 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 connection with an electronic device having a memory, characterized by: the computer program being executable by a processor to implement the auto park path optimization method of any one of claims 1-12.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211190937.0A CN115456297B (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 CN115456297B (en) | 2022-09-28 | 2022-09-28 | Automatic parking path optimization method and device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115456297A CN115456297A (en) | 2022-12-09 |
| CN115456297B true CN115456297B (en) | 2024-05-21 |
Family
ID=84306388
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211190937.0A Active CN115456297B (en) | 2022-09-28 | 2022-09-28 | Automatic parking path optimization method and device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115456297B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117533354B (en) * | 2023-12-28 | 2024-04-02 | 安徽蔚来智驾科技有限公司 | Track generation method, driving control method, storage medium and intelligent device |
Citations (9)
| 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 |
| 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 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10571921B2 (en) * | 2017-09-18 | 2020-02-25 | Baidu Usa Llc | Path optimization based on constrained smoothing spline for autonomous driving vehicles |
| US11186276B2 (en) * | 2018-07-27 | 2021-11-30 | Baidu Usa Llc | Adjusting speeds along a path for autonomous driving vehicles |
-
2022
- 2022-09-28 CN CN202211190937.0A patent/CN115456297B/en active Active
Patent Citations (9)
| 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 |
| 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 (3)
| Title |
|---|
| 全自动平行泊车路径规划方法研究;赵海兰;高松;孙宾宾;武哲;唐鹏;;科学技术与工程;20170308(07) * |
| 海洋平台结构动力响应优化设计与灵敏度分析;顾元宪, 马红艳, 姜成, 亢战, 张洪武;海洋工程;20010430(01) * |
| 海洋平台结构动力响应优化设计与灵敏度分析;顾元宪等;《海洋工程》;第19卷(第1期);第7-9页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115456297A (en) | 2022-12-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107063280B (en) | Intelligent vehicle path planning system and method based on control sampling | |
| CN112378408B (en) | Path planning method for realizing real-time obstacle avoidance of wheeled mobile robot | |
| Wang et al. | Collision-free navigation of autonomous vehicles using convex quadratic programming-based model predictive control | |
| CN115456297B (en) | Automatic parking path optimization method and device | |
| Muller et al. | Continuous curvature trajectory design and feedforward control for parking a car | |
| CN112622932B (en) | Automatic driving track-changing planning algorithm based on heuristic search of potential energy field | |
| JP5212753B2 (en) | Radar apparatus and measurement method used in the radar apparatus | |
| CN111650929B (en) | Self-adaptive sliding mode control method and system and mobile robot controller | |
| Xiong et al. | Application improvement of A* algorithm in intelligent vehicle trajectory planning | |
| JP6708535B2 (en) | Vehicle trajectory graph generator | |
| CN115454062A (en) | Robot dynamic path planning method and system based on Betz curve | |
| JP7019885B2 (en) | Vehicle control value determination device, vehicle control value determination method, and program | |
| CN114371709A (en) | Path planning method, device, storage medium and program product | |
| CN111857148B (en) | Unstructured road vehicle path planning method | |
| CN117141520B (en) | Real-time track planning method, device and equipment | |
| CN116518977A (en) | Unmanned aerial vehicle path planning method, system and equipment | |
| CN115534998A (en) | Automatic driving integrated decision-making method and device, vehicle and storage medium | |
| CN111781920A (en) | Automatic driving method, device and storage medium | |
| CN117170355A (en) | AGV trolley obstacle avoidance path planning method, device and equipment | |
| CN115891982A (en) | Automatic parking path planning method based on quadratic programming and parallel iteration | |
| Daniel et al. | Energy constrained trajectory generation for ADAS | |
| Jiang et al. | Multi-target tracking algorithm of vehicle-mounted millimeter-wave radar based on IMM-JPDA-STUKF | |
| CN114879665A (en) | Safe and energy-saving motion planning method and system for vehicle remote calling system | |
| Devaragudi et al. | MPC-based control of autonomous vehicles with localized path planning for obstacle avoidance under uncertainties | |
| Zhang et al. | Path Planning for Autonomous Driving with Curvature-considered Quadratic Optimization |
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 | ||
| GR01 | Patent grant |