CN113306549A - Automatic parking trajectory planning algorithm - Google Patents
Automatic parking trajectory planning algorithm Download PDFInfo
- Publication number
- CN113306549A CN113306549A CN202110675702.XA CN202110675702A CN113306549A CN 113306549 A CN113306549 A CN 113306549A CN 202110675702 A CN202110675702 A CN 202110675702A CN 113306549 A CN113306549 A CN 113306549A
- Authority
- CN
- China
- Prior art keywords
- parking
- vehicle
- point
- track
- parking space
- 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.)
- Granted
Links
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 58
- 238000004088 simulation Methods 0.000 claims abstract description 10
- 238000000034 method Methods 0.000 claims description 36
- 230000008569 process Effects 0.000 claims description 25
- 238000001514 detection method Methods 0.000 claims description 17
- 230000001133 acceleration Effects 0.000 claims description 3
- 238000013507 mapping Methods 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 230000008859 change Effects 0.000 abstract description 4
- 238000011065 in-situ storage Methods 0.000 abstract description 4
- 238000004364 calculation method Methods 0.000 description 16
- 238000010586 diagram Methods 0.000 description 5
- 238000011160 research Methods 0.000 description 5
- 230000007246 mechanism Effects 0.000 description 4
- 238000004898 kneading Methods 0.000 description 3
- 238000005457 optimization Methods 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008092 positive effect Effects 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 230000001502 supplementing effect Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
- B60W30/06—Automatic manoeuvring for parking
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
- B60W30/08—Active safety systems predicting or avoiding probable or impending collision or attempting to minimise its consequences
- B60W30/095—Predicting travel path or likelihood of collision
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
Landscapes
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
- Traffic Control Systems (AREA)
- Steering Control In Accordance With Driving Conditions (AREA)
Abstract
The invention provides an automatic parking trajectory planning algorithm, S1: determining a travelable area space; s2: selecting a target parking space and establishing a parking space coordinate system; s3: determining a plurality of key points between a starting point of the vehicle and a target point of the vehicle parking position through a simulation planning algorithm; s4: parking is carried out; s5: and finishing parking until the starting point of the vehicle is superposed with the target point of the parking position of the vehicle. The invention selects and calculates the key points of the parking track through a simulation planning algorithm, generates the parking track in segments, and analyzes the feasibility of the track according to the size of the parking space, the position of the vehicle body and the conditions of surrounding obstacles; therefore, the planning of the path is realized quickly. And a smooth and controllable parking track point meeting collision constraint is formed between two adjacent key points by adopting a track planning algorithm and combining interpolation, so that the final track is smooth and controllable, the curvature change rate is continuous, and the problem that the steering wheel is used for wearing tires when a vehicle is used in situ is solved.
Description
Technical Field
The invention belongs to the technical field of automatic driving track planning, and particularly relates to an automatic parking track planning algorithm.
Background
The automatic parking is an important function in automatic driving, partial commercial vehicles with autonomous parking are already available on the market at present, the traditional autonomous parking trajectory planning method mainly comprises two links, a parking map is divided into grids, a parking path is calculated in the grids by using search algorithms such as A, state lattices and the like, the path speed is calculated according to the parking path by using an optimization method, and the parking trajectory is finally formed.
The other method for calculating the parking track is an optimization method based on a vehicle kinematic model, and the method combines two steps of path planning and speed matching by using optimal control to directly calculate the parking track; however, the algorithm can be used in practice to cause the following problems:
(1) because the sensing range of the sensor is limited, the existing looking-around camera and ultrasonic scheme adopted by most automatic parking is biased, the looking-around visual range is generally within 10m, but the image distortion of the actual range larger than 6m is serious, the effective detection range is within 6m, and the ultrasonic wave is generally within 5m, so that the surrounding environment of a car body can not be sensed completely when a parking space is searched, obstacle detection and secondary parking space identification can be carried out in the parking process of a car, and once the parking space identification result is different from the result of the first parking space search, the position of a target parking space is required to be updated again, and the parking track is required to be planned again. Meanwhile, wheel speed odometer errors and vehicle transverse and longitudinal control errors caused by vehicle tire wear, tire pressure change and the like can require readjustment of parking tracks in the parking process.
(2) The geometric parking track algorithm of the circular arc and the straight line has the problems that the curvature of the track at the joint of the circular arc and the straight line is discontinuous, the steering wheel shakes during vehicle control, the steering wheel is driven after the vehicle control needs to be stopped, and the steering wheel moves after the steering wheel is executed to a target angle, so that the tire is abraded by driving the steering wheel in situ greatly.
(3) And vehicles can not be parked in narrow parallel parking spaces.
Disclosure of Invention
The invention provides an automatic parking track planning algorithm, which is characterized in that key points of a parking track are selected through a simulation planning algorithm, the key points are arranged according to a time sequence, then a current position of a vehicle body is taken as a starting point, the next key point is taken as a key point, the parking track is generated in a subsection mode, and then the feasibility of the track is analyzed according to the size of the parking space, the position of the vehicle body and the condition of surrounding obstacles; therefore, the planning of the path is realized quickly. A smooth and controllable parking track point meeting collision constraint is formed between two adjacent key points by adopting a track planning algorithm and combining interpolation, so that the final track is smooth and controllable, the curvature change rate is continuous, and the problem that the tires are abraded when the steering wheel is used for driving a vehicle in situ is solved; meanwhile, the planning algorithm can realize the parking of ultra-narrow parallel parking spaces.
In order to solve the technical problems, the invention adopts the technical scheme that: an automatic parking trajectory planning algorithm, comprising the steps of:
s1: determining a travelable space
S2: selecting a target parking space, establishing a parking space coordinate system, acquiring target parking space information, calculating coordinates of a vehicle starting point, and determining coordinates of a target at a vehicle parking position according to the parking space information;
s3: determining a plurality of key points between a starting point of the vehicle and a target point of the vehicle parking position through a simulation planning algorithm;
s4: taking a key point adjacent to the starting point of the vehicle as a current target point, moving the vehicle from the starting point to the target point, continuously updating the next key point as a new target point when the starting point of the vehicle is consistent with the current target point, moving to enable the starting point of the vehicle to move to the new target point, and continuously parking; if the current vehicle starting point is inconsistent with the current target point through movement, returning to the step S3;
s5: and finishing parking until the starting point of the vehicle is superposed with the target point of the parking position of the vehicle.
Further, the step S1 specifically includes the following steps:
s11, identifying and acquiring the obstacle information around the vehicle body;
s12, identifying and acquiring parking space information;
s13: and mapping the obstacle information and the parking space information around the vehicle body to a unified coordinate system to determine the vehicle driving space.
In the technical scheme, the vehicle driving space is a track range within which the vehicle can park, namely, the space range within which the vehicle can drive can be obtained after the obstacle information and the parking space information are unified into a coordinate system with the same reference point.
Further, in step S2, the target parking space information includes coordinates of a corner of the target parking space, and a length and a width of the parking space, where the parking space coordinate system is a coordinate system established with a vertex of the target parking space as an origin of coordinates.
In the technical scheme, the coordinates of the parking position of the vehicle can be determined by combining the coordinates of the angular point of the target parking space, the length and the width of the parking space and the coordinates of the initial point of the vehicle, and then the coordinates of the key point in the parking track are calculated according to the parameters.
Further, the simulation planning algorithm in the step S3 is to calculate the end point of each segment of the trajectory in segments as the key point by using a method of common tangent between an arc and a straight line and combining geometric space constraint conditions that the vehicle can travel.
Further, in step S3, the geometric space constraint condition that the vehicle can travel is that the arc radius of each track is equal to the minimum turning radius of the vehicle, and the vehicle does not collide with the parking space boundary and the road boundary in each track. The minimum turning radius of the vehicle is the turning radius of the vehicle in a left dead driving state or a right dead driving state.
Further, in the step S4, a plurality of specific trajectory points between two key points are generated according to a segmented trajectory point generation algorithm, so as to generate a continuous parking trajectory; when parking, the vehicle moves from the starting point to the target point along the parking track.
Further, in step S4, the process of generating the parking trajectory points by the segment trajectory point generating algorithm is as follows:
s411, generating a straight line circular arc splicing line segment through a track planning algorithm, and then generating discrete track points according to the length of the line segment and the position of a curvature catastrophe point by non-equal-interval sampling;
s412, taking the discrete track points as control points, generating a smooth and continuous curve through an interpolation algorithm, and then dispersing to obtain track points at intervals of 0.1 m;
s413, calculating the parking track length to generate a smooth speed curve according to the parking speed and the acceleration limit, and dispersing to obtain the target speed of each track point;
s414, connecting a plurality of track points according to the target speed of the track points, and finally generating a smooth parking track.
Further, in step S1, a detection module is disposed on the periphery of the vehicle body to identify and acquire obstacle information and parking space information around the vehicle body, wherein the detection module is a camera or an ultrasonic module disposed on the periphery of the vehicle body, or a combination thereof.
Further, in step S4, the detection module monitors the obstacle information around the vehicle body in real time during the parking process to determine whether a new parking trajectory needs to be re-planned during the parking process, which includes the following steps:
(1) the vehicle moves along with the parking track points, the detection module identifies the obstacle information around the vehicle body in real time in the moving process, and the obstacle information is mapped into a parking space coordinate system;
(2) braking the vehicle to prevent a collision if the obstacle enters the travelable space in step S1; detecting whether the obstacle exists continuously after 10s, and if the obstacle disappears, continuing parking; returning to step S3 if the obstacle continues to exist;
(3) when the vehicle reaches the next key point position, the detection module carries out parking space secondary identification, and compares the identified parking space result with the current target parking space to judge whether to update the parking space;
(4) and if the parking space is updated, returning to the step S1.
The invention has the advantages and positive effects that:
1. the method comprises the steps of selecting key points for calculating the parking track through a simulation planning algorithm, arranging the key points according to a time sequence, generating the parking track in segments by taking the current position of a vehicle body as a starting point and the next key point as a key point, and analyzing the feasibility of the track according to the size of the parking space, the position of the vehicle body and the conditions of surrounding obstacles; therefore, the planning of the path is realized quickly.
2. According to the algorithm, a smooth and controllable parking track point is formed between two adjacent key points by adopting a track planning algorithm and combining an interpolation algorithm, so that the final track is smooth and controllable, the curvature change rate is continuous, and the problem that a steering wheel is used for a vehicle in situ to wear tires is solved.
3. The algorithm supports the minimum parallel parking space to be the vehicle body length plus 0.8m parking space through accurate calculation, so that the problem of parallel parking in an ultra-narrow parking space can be solved.
Drawings
FIG. 1 is a schematic view of the Ackerman steering mechanism;
FIG. 2 is a schematic view of the geometric relationship between the coordinates of the vehicle contour points and the coordinates of the center point of the rear axle;
FIG. 3 is a schematic diagram I of a vertical parking trajectory planning in an automatic parking trajectory planning algorithm according to the present invention;
FIG. 4 is a schematic diagram of a vertical parking trajectory planning in an automatic parking trajectory planning algorithm according to a second embodiment of the present invention;
FIG. 5 is a schematic diagram of a parallel parking entry trajectory planning and a parallel parking exit trajectory planning in the automatic parking trajectory planning algorithm according to the present invention;
FIG. 6 is an overall flow chart of an automatic parking trajectory planning algorithm of the present invention;
FIG. 7 is a flow chart of an algorithm for generating a segmented track point in an automatic parking track planning algorithm according to the present invention;
fig. 8 is a flow chart of re-planning a parking trajectory in an automatic parking trajectory planning algorithm according to the present invention.
Detailed Description
The following detailed description of embodiments of the invention refers to the accompanying drawings.
As shown in fig. 6, the automatic parking trajectory planning algorithm provided by the present invention includes the following steps:
s1: determining a travelable space
S11, identifying and acquiring the obstacle information around the vehicle body;
s12, identifying and acquiring parking space information;
s13: mapping the obstacle information and the parking space information around the vehicle body to a unified coordinate system, and determining a vehicle driving space;
s2: selecting a target parking space, establishing a parking space coordinate system, acquiring target parking space information, calculating coordinates of a vehicle starting point, and determining coordinates of a target at a vehicle parking position according to the parking space information;
s3: determining a plurality of key points between a starting point of the vehicle and a target point of the vehicle parking position through a simulation planning algorithm;
s4: taking a key point adjacent to the starting point of the vehicle as a current target point, moving the vehicle from the starting point to the target point, continuously updating the next key point as a new target point when the starting point of the vehicle is consistent with the current target point, moving to enable the starting point of the vehicle to move to the new target point, and continuously parking; if the current vehicle starting point is inconsistent with the current target point through movement, returning to the step S3;
s5: and finishing parking until the starting point of the vehicle is superposed with the target point of the parking position of the vehicle.
In step S13, the vehicle drivable space is a trajectory range in which the vehicle can park, that is, the space range in which the vehicle can drive can be obtained after the obstacle information and the parking space information are unified into a coordinate system with the same reference point. In the step S2, the target parking space information includes coordinates of the corner of the target parking space, and a length and a width of the parking space, where the parking space coordinate system is a coordinate system established with a vertex of the target parking space as an origin of coordinates.
The simulation planning algorithm in the step S3 is to calculate the end point of each segment of the trajectory in segments as the key point by using a method of common tangent between an arc and a straight line and combining geometric space constraint conditions that the vehicle can travel. In the step S3, the geometric space constraint condition that the vehicle can travel is that the arcs of two adjacent key point tracks are tangent, the arc radius of each track section is equal to the minimum turning radius of the vehicle, and the vehicle in each track section does not collide with the parking space boundary and the road boundary. Wherein, the minimum turning radius is the turning radius when the vehicle is dead left or dead right.
In the concrete calculation, the circular arc section is calculated by adopting the minimum turning radius, if the vehicle cannot back for warehousing at one time due to the limitation of parking space or road boundary, the vehicle needs to back for warehousing again after driving forward for a certain distance, so that a warehouse kneading action is formed, the tracks of forward and backward in the warehouse kneading process are in a circumscribed relation, the circular arcs or straight lines are in a circumscribed relation until the final target position can be parked in the warehouse kneading process at the last time, and the position coordinates and angles of all key points in the middle are calculated. Wherein the minimum turning radius is the radius of the track of the left dead or right dead turn of the vehicle;
in the invention, the coordinate parameters of the vehicle are determined by adopting the Ackerman steering mechanism principle, wherein the Ackerman steering mechanism principle diagram is shown in figure 1; as shown in fig. 2, it is a schematic diagram of the geometric relationship between the coordinates of the vehicle contour points and the coordinates of the center point of the rear axle:
the Ackerman steering mechanism comprises the following specific principles: under the condition of low speed, the vehicle has kinematic constraint and no sideslip phenomenon, and the equivalent steering angle of a front axle of the vehicle is
The motion track of the robot makes a circle-drawing motion with a circle center, and the radius of the circle is independent of the speed
Wherein L is the wheel base, R is the radius of the motion curve of the center point of the rear axle, and the curvature is ρ 1/R, as shown in fig. 1.
When the vehicle parameter structure parameter is fixed, the equivalent rotation angle of the front axle of the vehicle determines the shape of the driving path of the midpoint of the rear axle, and the coordinate of any point of the vehicle body can be obtained through the coordinate of the midpoint of the rear axle, the included angle between the tangent line of the path and the coordinate system and the vehicle structure parameter. Therefore, the middle point pose of the rear axle is selected as a research object in the inventionRecording the midpoint of the rear axle as M, and the M point position as [ x y theta ]]A, B, C, D Point coordinates are [ x ] respectivelyA yA]、[xB yB]、[xC yC]、[xD yD]. Wherein L is the length of the wheel base of the vehicle, LfIs the front overhang length of the vehicle, LrIs the rear overhang length, LkThe vehicle width belongs to vehicle structure parameters, is provided by a general host factory and can be measured by the host factory. The relationship between the vehicle contour vertex and the vehicle rear axle midpoint can be obtained by the geometric relationship between the vehicle structure parameters and the figure 2, and is shown in the formulas (1-1) to (1-4).
In the invention, the coordinates of the initial point of the vehicle are the initial point of the research point of the vehicle, and the target point of the parking position of the vehicle is the target point of the research point of the vehicle after the parking of the vehicle is finished. In the whole process, the same vehicle research point is used as a research object to carry out track planning.
In the step S4, generating a plurality of specific track points between two key points according to a segmented track point generating algorithm, thereby generating a continuous parking track; when parking, the vehicle moves from the starting point to the target point along the parking track.
As shown in fig. 7, the process of generating the parking trajectory points by the segment trajectory point generation algorithm is as follows:
s411, generating a straight line circular arc splicing line segment through a track planning algorithm, and then generating discrete track points according to the length of the line segment and the position of a curvature catastrophe point by non-equal-interval sampling;
s412, taking the discrete track points as control points, generating a smooth and continuous curve through an interpolation algorithm, and then dispersing to obtain track points at intervals of 0.1 m;
s413, calculating the parking track length to generate a smooth speed curve according to the parking speed and the acceleration limit, and dispersing to obtain the target speed of each track point; wherein the velocity calculation formula is:
and S414, connecting the plurality of track points to finally generate a smooth parking track.
Wherein the content of the first and second substances,in step S411The path planning algorithm determines a path by planning a dubins curve, so as to generate a specific straight line circular arc splicing line segment, and the interpolation algorithm in the step S412 generates a smooth continuous curve by adopting a tri-Spline interpolation in the Cubic B-Spline interpolation.
The purpose of planning the dubins curve is to find a curve which has the shortest distance from a starting point to a target point and meets the turning radius, the advancing direction, the initial relative position and the speed direction; the dubins path type is composed of a straight line (S), a left turn (L) and a right turn (R), and the dubins path type is totally 6 types: { RSR, RSL, LSR, LSL, RLR, LRL }.
In this embodiment, in step S1, a detection module is provided on the periphery of the vehicle body to identify and acquire obstacle information and parking space information around the vehicle body, and the detection module is a camera or an ultrasonic module provided on the periphery of the vehicle body, or a combination of the two. As shown in fig. 8, in step S4, the detection module monitors the obstacle information around the vehicle body in real time during parking to determine whether a new parking trajectory needs to be re-planned during parking, which includes the following steps:
(1) the vehicle moves along with the parking track points, the detection module identifies the obstacle information around the vehicle body in real time in the moving process, and the obstacle information is mapped into a parking space coordinate system;
(2) braking the vehicle to prevent a collision if the obstacle enters the travelable space in step S1; detecting whether the obstacle exists continuously after 10s, and if the obstacle disappears, continuing parking; returning to step S3 if the obstacle continues to exist;
(3) when the vehicle reaches the next key point position, the detection module carries out parking space secondary identification, and compares the identified parking space result with the current target parking space to judge whether to update the parking space;
(4) and if the parking space is updated, returning to the step S1.
In the practical application process, the calculation of the key points of the vertical parking tracks, the calculation of the key points of the parallel parking garage entrance tracks and the calculation of the key points of the parallel parking exit tracks can be divided according to the position of the target parking space and the length of the practical parking space.
The following embodiments 1 to 3 respectively describe steps and specific algorithm processes for calculating key points of a vertical parking trajectory, calculating key points of a parallel parking trajectory, and calculating key points of a parallel parking trajectory.
Example 1:
in this embodiment, the calculation of key points of a vertical parking trajectory is described, and the trajectory refers to fig. 3, and the specific steps are as follows:
the first step is as follows: backing up and warehousing, namely knowing that the tail of a vehicle just collides with the boundary of the parking space, ensuring that the vehicle does not touch the right vertex angle of the parking space, wherein the motion track in the process is an arc P1P 2;
wherein, define P1The coordinates of the point are (x)p1,yp1,θp1) Wherein y isp1Is confirmed to be known after the parking space is searched, thetap1For initial angle set to 0 degrees, mainly solve for xp1；
Known line segment O1P1Is the turning radius R, E is the outer side point of the right rear wheel of the vehicle, H is O1P1The intersection with the x-axis is obtained from the geometric relationship:
O1O＝O1E1＝O1P1-Lk/2＝R-Lk/2
O1H＝O1P1-P1H＝R-yp1
then, solve for P2Coordinates of points (x)p2,yp2,θp2) First, solve for thetap2；
Assuming that the rear of the vehicle collides with the left boundary (or left boundary extension line) of the parking space, as shown in fig. 4, knowing the width w of the parking space, it can be obtained according to the geometrical relationship:
xD＝-w
O1D＝R+Lk/2
θp2＝θD＝arcsin((xp1-xD)/O1D)＝arcsin((xp1+w)/(R+Lk/2))
yD＝O1D×cosθp2-O1H＝(R+Lk/2)×cosθp2-(R-yp1)
obtaining the coordinate point P from the formulas 1-42The relationship to D is as follows:
the second step is that: the vehicle advances, the angle is adjusted to make the course angle approach 90 DEG, the motion track is as the arc P in figure 42P3；
The third step: the vehicle backs to the center of the parking space, the target course angle is 90 degrees, and the motion track is an arc P3P4；
Wherein, according to P2Coordinates of points (x)p2,yp2,θp2) Can easily solve O2Coordinates are as follows:
xO2＝xp2-R sinθp2
yO2＝yp2+R cosθp2
at the same time, due to O2And O3Tangent, O3Tangent to the center line of the parking space, the following relationship exists:
xO3＝-w/2+R
xp3＝(xO3+xO2)/2
yp3＝(yO3+yO2)/2
θp3＝arcsin((xO3-xO2)/2R)
xp4＝-w/2
yp4＝yO3
θp4＝π/2
the fourth step: the vehicle continues to back to the final target position, is a straight line segment, so that the vehicle head is flush with the top of the parking space, and the running track of the vehicle is P4P5；
xp5＝-w/2
yp5＝-L-Lf
θp5＝π/2
In the calculation of the key points of the vertical parking trajectory in the embodiment, each section of trajectory needs to be satisfied, and the vehicle does not collide with the parking space boundary and the road boundary; radius of arc equal to minimum turning radius of vehicle, O1And O2Tangent, O2And O3Tangent, O3Is tangent with the central line of the parking space.
Example 2:
in this embodiment, the calculation of key points of a parallel parking garage entrance trajectory is described, where the trajectory may refer to fig. 5, the calculation of the key points of the parallel parking garage entrance trajectory adopts a reverse push algorithm, and it is assumed that how a vehicle needs to park out of a parking space in the parking space and does not collide with a boundary of the parking space, so as to push out the key points of the parking trajectory, and the parking process is the reverse process of the parking out process; the method comprises the following steps:
assuming a horizontal berthing-out starting point position P1If the course angle is 0, P2For vehicles along O1The position of the arc when it is in contact with the right boundary of the parking space, P, in a similar manner3For vehicles from P2Position along O2The circular arc backs to the position contacted with the left boundary of the parking space, and the circular arc pushes inwards in sequence until PnWhen the vehicle can be parked out of the parking space directly without colliding with the boundary of the parking space, (n is 2k, k is 1,2, 3..) the parked track points are sequentially P1，P2，P3，...PnTherefore, the key points of the multi-step horizontal berthing track are as follows: pn，Pn-1，...，P3，P2，P1。
The calculation process is as follows:
firstly, setting the final target point position of the vehicle when the vehicle is parked into the parking space, and knowing that the depth of the parallel parking space is d, the length is c, and P1 (x)p1,yp1,θp1) Setting the coordinate position of the final target position as follows:
xp1＝-c-L/2
yp1＝-d/2
θp1＝0
simulating the process of parking and taking out of the parking space with the starting point P1When the steering wheel is left dead, the steering wheel is opened and the vehicle is driven to P2When the vehicle collides with the right boundary of the parking space, the steering wheel is driven to reverse until reaching the position P3And the vehicle collides with the left boundary of the parking space or the bottom line of the parking space, and the vehicle can be successfully driven out of the parking space after the vehicle is not collided with the right boundary in the left-opening dead-moving process.
P1After the position of P is known, P can be easily solved according to the constraint condition2Due to P2When the vehicle right vertex B just collides with the right boundary of the parking space, the following relationship exists:
xB＝0
yB<0
the vehicle can be driven from P using the approximation1Calculating the vertex B coordinate of the new position every 0.1m advance of the position, if x is satisfiedB<0,yB<0 then continue to accumulateUntil x is satisfiedB>0,yB<0 is stopped, the last position is P2Position of coordinate points, from P2Backing to position P3The constraint conditions are also satisfied:
yC>-d
xD>-c
p can be obtained using approximation3Coordinate point, and so on, when PnSatisfy xB>0,yB>When 0, the vehicle can safely leave the parking space, and the calculated coordinate position is reversed to obtain a key point P for horizontal parkingn,Pn-1,...,P3,P2,P1。
Supplementing: known coordinate point P1(xp1,yp1,θp1) And a new coordinate point position calculation process after 0.1m of advance:
θ＝0.1/R+θp1
x＝xp1+R sinθ-R sinθp1
y＝yp1+R cosθp1-R cosθ
in the calculation of key points of multi-step parallel parking garage entering tracks, the following conditions need to be met: the vehicle does not collide with the parking space boundary and the road boundary; the radius of the circular arc is the minimum turning radius of the vehicle, each section of track is in a tangent relation, and each point of the vehicle outline makes concentric circle motion with the same center of circle, but the radius is different.
Example 3:
this embodiment describes the parallel-parked trajectory key point calculation, which has the same principle as the parallel-parked trajectory key point calculation of embodiment 2, and the trajectory refers to fig. 5, and the steps are as follows:
according to the position P of the ultrasonic radar or the camera measuring the horizontal berthing starting point1If the default course angle is 0, then P2For vehicles along O1The position of the arc when it is in contact with the right boundary of the parking space, P, in a similar manner3For vehicles from P2Position along O2The circular arc backs until the position of the left boundary of the parking space is contacted, and the circular arc pushes inwards in sequence until the position is Pn(n＝2k, k is 1,2,3, once) the vehicle can directly park out the parking stall and can not collide with the parking stall boundary, and the track point of parking out is P in proper order1，P2，P3，...Pn；
In parallel berthing trajectory key point calculation, the following conditions need to be satisfied: the vehicle does not collide with the parking space boundary and the road boundary; the radius of the circular arc is the minimum turning radius of the vehicle, each section of track is in a tangent relation, and each point of the vehicle outline makes concentric circle motion with the same center of circle, but the radius is different.
While specific embodiments of the present invention have been described in detail, the description is merely illustrative of the preferred embodiments of the present invention and is not to be construed as limiting the scope of the invention. All equivalent changes and modifications made within the scope of the present invention shall fall within the scope of the present invention.
Claims (9)
1. An automatic parking trajectory planning algorithm is characterized in that: the method comprises the following steps:
s1: determining a travelable space;
s2: selecting a target parking space, establishing a parking space coordinate system, acquiring target parking space information, calculating coordinates of a starting point of a vehicle, and determining coordinates of a target point of a parking position of the vehicle according to the target parking space information;
s3: determining a plurality of key points between a starting point of the vehicle and a target point of the vehicle parking position through a simulation planning algorithm;
s4: taking a key point adjacent to the starting point of the vehicle as a current target point, moving the vehicle from the starting point to the target point, continuously updating the next key point as a new target point when the starting point of the vehicle is consistent with the current target point, moving to enable the starting point of the vehicle to move to the new target point, and continuously parking; if the current vehicle starting point is inconsistent with the current target point through movement, returning to the step S3;
s5: and finishing parking until the starting point of the vehicle is superposed with the target point of the parking position of the vehicle.
2. The automated parking trajectory planning algorithm of claim 1, wherein: the step S1 specifically includes the following steps:
s11, identifying and acquiring the obstacle information around the vehicle body;
s12, identifying and acquiring parking space information;
s13: and mapping the obstacle information and the parking space information around the vehicle body to a unified coordinate system to determine the vehicle driving space.
3. The automated parking trajectory planning algorithm of claim 2, wherein: in the step S2, the target parking space information includes coordinates of corner points of the target parking space, and a length and a width of the target parking space.
4. The automated parking trajectory planning algorithm of claim 2, wherein: the simulation planning algorithm in the step S3 is to calculate the end point of each segment of the trajectory in segments as the key point by using a method of common tangent between an arc and a straight line and combining geometric space constraint conditions that the vehicle can travel.
5. The automated parking trajectory planning algorithm of claim 4, wherein: in step S3, the geometric space constraint condition that the vehicle can travel is that the arc radius of each track segment is equal to the minimum turning radius of the vehicle, and the vehicle does not collide with the parking space boundary and the road boundary in each track segment.
6. The automated parking trajectory planning algorithm of claim 1, wherein: in the step S4, generating a plurality of specific track points between two key points according to a segmented track point generating algorithm, thereby generating a continuous parking track; when parking, the vehicle moves from the starting point to the target point along the parking track.
7. An automated parking trajectory planning algorithm according to claim 6, wherein: in the step S4, the process of generating the parking trajectory points by the segmented trajectory point generating algorithm is as follows:
s411, generating a straight line circular arc splicing line segment through a track planning algorithm, and then generating discrete track points according to the length of the line segment and the position of a curvature catastrophe point by non-equal-interval sampling;
s412, taking the discrete track points as control points, generating a smooth and continuous curve through an interpolation algorithm, and then dispersing to obtain track points at intervals of 0.1 m;
s413, calculating the parking track length to generate a smooth speed curve according to the parking speed and the acceleration limit, and dispersing to obtain the target speed of each track point;
and S414, connecting the plurality of track points to finally generate a smooth parking track.
8. The automated parking trajectory planning algorithm of claim 1, wherein: in step S1, a detection module is disposed on the periphery of the vehicle body to identify and acquire obstacle information and parking space information around the vehicle body, and the detection module is a camera or an ultrasonic module disposed on the periphery of the vehicle body, or a combination thereof.
9. The automated parking trajectory planning algorithm of claim 7, wherein: in the step S4, the detection module monitors the obstacle information around the vehicle body in real time during the parking process to determine whether a new parking trajectory needs to be re-planned during the parking process, which includes the following steps:
(1) the vehicle moves along with the parking track points, the detection module identifies the obstacle information around the vehicle body in real time in the moving process, and the obstacle information is mapped into a parking space coordinate system;
(2) braking the vehicle to prevent a collision if the obstacle enters the travelable space in step S1; detecting whether the obstacle exists continuously after 10s, and if the obstacle disappears, continuing parking; returning to step S3 if the obstacle continues to exist;
(3) when the vehicle reaches the next key point position, the detection module carries out parking space secondary identification, and compares the identified parking space result with the current target parking space to judge whether to update the parking space;
(4) and if the parking space is updated, returning to the step S1.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110675702.XA CN113306549B (en) | 2021-06-17 | 2021-06-17 | Automatic parking trajectory planning algorithm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110675702.XA CN113306549B (en) | 2021-06-17 | 2021-06-17 | Automatic parking trajectory planning algorithm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113306549A true CN113306549A (en) | 2021-08-27 |
| CN113306549B CN113306549B (en) | 2022-11-01 |
Family
ID=77379477
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110675702.XA Active CN113306549B (en) | 2021-06-17 | 2021-06-17 | Automatic parking trajectory planning algorithm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113306549B (en) |
Cited By (18)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113696886A (en) * | 2021-09-17 | 2021-11-26 | 中国第一汽车股份有限公司 | Parking track automatic determination method, device, medium and electronic equipment |
| CN113799768A (en) * | 2021-09-27 | 2021-12-17 | 惠州华阳通用智慧车载系统开发有限公司 | Automatic parking method based on vertical parking spaces |
| CN113887060A (en) * | 2021-10-13 | 2022-01-04 | 英博超算(南京)科技有限公司 | Novel vehicle positioning algorithm of automatic parking system |
| CN113985889A (en) * | 2021-11-11 | 2022-01-28 | 北京三快在线科技有限公司 | Trajectory planning method and device, storage medium and electronic equipment |
| CN114001745A (en) * | 2021-10-25 | 2022-02-01 | 东南大学 | Electric automobile parking guide system and position detection and evaluation method thereof |
| CN114013428A (en) * | 2021-11-29 | 2022-02-08 | 江苏大学 | Dynamic parking path planning method based on intermolecular acting force |
| CN114030463A (en) * | 2021-11-23 | 2022-02-11 | 上海汽车集团股份有限公司 | Path planning method and device of automatic parking system |
| CN114103934A (en) * | 2021-12-23 | 2022-03-01 | 岚图汽车科技有限公司 | Automatic parking path planning method, device and equipment and readable storage medium |
| CN114132304A (en) * | 2021-10-13 | 2022-03-04 | 武汉光庭信息技术股份有限公司 | Speed planning method and device in autonomous parking |
| CN114248761A (en) * | 2021-12-10 | 2022-03-29 | 重庆长安汽车股份有限公司 | Target track discretization method, parking path tracking error optimization method and system |
| CN114274952A (en) * | 2021-12-24 | 2022-04-05 | 重庆长安汽车股份有限公司 | Vertical parking space autonomous parking method and system, storage medium and electronic device |
| CN114516322A (en) * | 2022-02-24 | 2022-05-20 | 重庆长安汽车股份有限公司 | System and method for parking out vehicle, vehicle and computer storage medium |
| CN114701875A (en) * | 2022-03-08 | 2022-07-05 | 中国人民解放军空军工程大学 | Guiding device and method for loading and unloading of railway vehicle under light condition |
| CN115456297A (en) * | 2022-09-28 | 2022-12-09 | 远峰科技股份有限公司 | Automatic parking path optimization method and device |
| CN115489515A (en) * | 2022-08-29 | 2022-12-20 | 武汉理工大学 | Automatic parking trajectory planning method for narrow vertical parking space and electronic equipment |
| CN113799766B (en) * | 2021-09-17 | 2023-04-18 | 东风汽车集团股份有限公司 | Constrained parallel parking path planning method, electronic device and storage medium |
| WO2023123711A1 (en) * | 2021-12-28 | 2023-07-06 | 魔门塔(苏州)科技有限公司 | Automatic parking path planning method and apparatus, medium and device |
| CN116442992A (en) * | 2023-06-15 | 2023-07-18 | 广汽埃安新能源汽车股份有限公司 | Parking control method and device |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1987357A (en) * | 2006-12-26 | 2007-06-27 | 浙江工业大学 | Intelligent parking auxiliary device based on omnibearing computer sight |
| CN109866761A (en) * | 2019-03-05 | 2019-06-11 | 重庆长安汽车股份有限公司 | Automatic parking paths planning method, system and computer readable storage medium |
| US20200398867A1 (en) * | 2019-06-24 | 2020-12-24 | Honda Motor Co., Ltd. | Parking assist system |
| CN112537294A (en) * | 2020-12-23 | 2021-03-23 | 上汽通用汽车有限公司 | Automatic parking control method and electronic equipment |
| CN112829740A (en) * | 2019-11-25 | 2021-05-25 | 湖北亿咖通科技有限公司 | Vehicle control method and device for autonomous parking and vehicle |
-
2021
- 2021-06-17 CN CN202110675702.XA patent/CN113306549B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1987357A (en) * | 2006-12-26 | 2007-06-27 | 浙江工业大学 | Intelligent parking auxiliary device based on omnibearing computer sight |
| CN109866761A (en) * | 2019-03-05 | 2019-06-11 | 重庆长安汽车股份有限公司 | Automatic parking paths planning method, system and computer readable storage medium |
| US20200398867A1 (en) * | 2019-06-24 | 2020-12-24 | Honda Motor Co., Ltd. | Parking assist system |
| CN112829740A (en) * | 2019-11-25 | 2021-05-25 | 湖北亿咖通科技有限公司 | Vehicle control method and device for autonomous parking and vehicle |
| CN112537294A (en) * | 2020-12-23 | 2021-03-23 | 上汽通用汽车有限公司 | Automatic parking control method and electronic equipment |
Cited By (29)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113696886A (en) * | 2021-09-17 | 2021-11-26 | 中国第一汽车股份有限公司 | Parking track automatic determination method, device, medium and electronic equipment |
| CN113799766B (en) * | 2021-09-17 | 2023-04-18 | 东风汽车集团股份有限公司 | Constrained parallel parking path planning method, electronic device and storage medium |
| WO2023040774A1 (en) * | 2021-09-17 | 2023-03-23 | 中国第一汽车股份有限公司 | Method for automatically determining parking trajectory and electronic device |
| CN113799768A (en) * | 2021-09-27 | 2021-12-17 | 惠州华阳通用智慧车载系统开发有限公司 | Automatic parking method based on vertical parking spaces |
| CN113887060B (en) * | 2021-10-13 | 2024-05-10 | 英博超算(南京)科技有限公司 | Novel automatic parking system vehicle positioning method |
| CN114132304A (en) * | 2021-10-13 | 2022-03-04 | 武汉光庭信息技术股份有限公司 | Speed planning method and device in autonomous parking |
| CN114132304B (en) * | 2021-10-13 | 2023-10-13 | 武汉光庭信息技术股份有限公司 | Speed planning method and device in autonomous parking |
| CN113887060A (en) * | 2021-10-13 | 2022-01-04 | 英博超算(南京)科技有限公司 | Novel vehicle positioning algorithm of automatic parking system |
| CN114001745A (en) * | 2021-10-25 | 2022-02-01 | 东南大学 | Electric automobile parking guide system and position detection and evaluation method thereof |
| CN114001745B (en) * | 2021-10-25 | 2023-11-24 | 东南大学 | Electric automobile parking guide system and position detection and evaluation method thereof |
| CN113985889B (en) * | 2021-11-11 | 2024-04-30 | 北京三快在线科技有限公司 | Track planning method and device, storage medium and electronic equipment |
| CN113985889A (en) * | 2021-11-11 | 2022-01-28 | 北京三快在线科技有限公司 | Trajectory planning method and device, storage medium and electronic equipment |
| CN114030463A (en) * | 2021-11-23 | 2022-02-11 | 上海汽车集团股份有限公司 | Path planning method and device of automatic parking system |
| CN114030463B (en) * | 2021-11-23 | 2024-05-14 | 上海汽车集团股份有限公司 | Path planning method and device for automatic parking system |
| CN114013428A (en) * | 2021-11-29 | 2022-02-08 | 江苏大学 | Dynamic parking path planning method based on intermolecular acting force |
| CN114248761A (en) * | 2021-12-10 | 2022-03-29 | 重庆长安汽车股份有限公司 | Target track discretization method, parking path tracking error optimization method and system |
| CN114248761B (en) * | 2021-12-10 | 2024-06-04 | 重庆长安汽车股份有限公司 | Target track discretization method, parking path tracking error optimization method and system |
| CN114103934A (en) * | 2021-12-23 | 2022-03-01 | 岚图汽车科技有限公司 | Automatic parking path planning method, device and equipment and readable storage medium |
| CN114274952B (en) * | 2021-12-24 | 2023-09-22 | 重庆长安汽车股份有限公司 | Autonomous parking method and system for vertical parking space, storage medium and electronic equipment |
| CN114274952A (en) * | 2021-12-24 | 2022-04-05 | 重庆长安汽车股份有限公司 | Vertical parking space autonomous parking method and system, storage medium and electronic device |
| WO2023123711A1 (en) * | 2021-12-28 | 2023-07-06 | 魔门塔(苏州)科技有限公司 | Automatic parking path planning method and apparatus, medium and device |
| CN114516322B (en) * | 2022-02-24 | 2024-04-26 | 重庆长安汽车股份有限公司 | System and method for parking vehicle, vehicle and computer storage medium |
| CN114516322A (en) * | 2022-02-24 | 2022-05-20 | 重庆长安汽车股份有限公司 | System and method for parking out vehicle, vehicle and computer storage medium |
| CN114701875A (en) * | 2022-03-08 | 2022-07-05 | 中国人民解放军空军工程大学 | Guiding device and method for loading and unloading of railway vehicle under light condition |
| CN115489515A (en) * | 2022-08-29 | 2022-12-20 | 武汉理工大学 | Automatic parking trajectory planning method for narrow vertical parking space and electronic equipment |
| CN115456297A (en) * | 2022-09-28 | 2022-12-09 | 远峰科技股份有限公司 | Automatic parking path optimization method and device |
| CN115456297B (en) * | 2022-09-28 | 2024-05-21 | 远峰科技股份有限公司 | Automatic parking path optimization method and device |
| CN116442992A (en) * | 2023-06-15 | 2023-07-18 | 广汽埃安新能源汽车股份有限公司 | Parking control method and device |
| CN116442992B (en) * | 2023-06-15 | 2023-09-05 | 广汽埃安新能源汽车股份有限公司 | Parking control method and device |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113306549B (en) | 2022-11-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113306549B (en) | Automatic parking trajectory planning algorithm | |
| CN109987092B (en) | Method for determining vehicle obstacle avoidance and lane change time and method for controlling obstacle avoidance and lane change | |
| CN109976329B (en) | Planning method for vehicle obstacle avoidance and lane change path | |
| CN107776570B (en) | Full-automatic parking method and full-automatic parking system | |
| CN111307152B (en) | Reverse generation planning method for autonomous parking path | |
| CN105197010B (en) | Auxiliary parking system and auxiliary are parked control method | |
| CN110696819B (en) | Automatic parking path planning method and system | |
| JP2022544715A (en) | Modeling and predicting yielding behavior | |
| Zheng et al. | RRT based path planning for autonomous parking of vehicle | |
| JP6989333B2 (en) | Parking support device | |
| CN104881025A (en) | Reactive navigation control method for underground mining vehicle | |
| CN101751039A (en) | Parking assist apparatus | |
| CN113895463B (en) | Path planning method suitable for turning around of automatic driving vehicle | |
| CN108759829B (en) | Local obstacle avoidance path planning method for intelligent forklift | |
| JP5666327B2 (en) | Unmanned moving body and control method of unmanned moving body | |
| JP6759143B2 (en) | Parking support device | |
| JP2014054912A (en) | Parking support system | |
| CN115195790B (en) | Method and device for predicting vehicle track, electronic equipment and storage medium | |
| CN113212424B (en) | Vehicle and automatic parking method and device thereof | |
| US11908204B2 (en) | Tracking articulated objects | |
| CN113110497A (en) | Navigation path-based edge obstacle-detouring path selection method, chip and robot | |
| CN113885525A (en) | Path planning method and system for automatically driving vehicle to get rid of trouble, vehicle and storage medium | |
| CN116331264A (en) | Obstacle avoidance path robust planning method and system for unknown obstacle distribution | |
| WO2023278152A1 (en) | Estimating angle of a vehicle wheel based on non-steering variables | |
| CN112363491B (en) | Robot turning control method and device |
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 | ||
| GR01 | Patent grant |